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ABSTRACT 


Autonomous  Underwater  Vehicles  (AUV)  currently  use  varying  methods  for 
navigation,  but  incorporating  GPS  into  those  methods  is  becoming  a  popular  technique. 
This  thesis  experimentally  evaluates  the  configuration  and  implementation  of  the 
Navigational  Suite  within  the  Naval  Postgraduate  School’s  AUV,  the  ARIES  (Acoustic 
Radio  Interactive  Exploratory  Server).  Specific  attention  is  given  to  the  configuration  of 
the  vehicle’s  newly  completed  DGPS  (Differential  Global  Positioning  System).  A  brief 
discussion  of  DGPS  and  Extended  Kalman  Filter  theory  continues  with  a  description  of 
the  make-up  and  applications  of  components  within  the  Suite.  Details  of  a  series  of 
experiments,  which  begins  with  evaluation  of  the  DGPS  setup,  then  qualifies  the  system 
in  an  open- water  environment,  and  finally  qualifies  the  DGPS  in  conjunction  with  newly 
configured  ARIES  Navigational  Filter,  provide  an  examination  of  the  Suite’s 
performance. 
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I.  INTRODUCTION 


A.  BACKGROUND 

The  number  of  missions  that  can  benefit  from  the  utilization  of  an  Autonomous 
Underwater  Vehicle  (AUV)  grows  every  day.  An  AUV  makes  it  possible  to  complete 
underwater  tasks,  with  minimal  support  and  supervision.  Potential  uses  for  the  vehicles 
include  arctic  under  ice  exploration,  offshore  oil  platform  examining,  ocean  feature 
mapping,  mine  countermeasure  operations,  and  underwater  object  location  and 
positioning  such  as  fiber-optic  cables  and  oil  pipelines.  It  is  also  important  to  realize 
these  missions  require  an  AUV  that  can  accurately  determine  its  geographic  position.  In 
most  instances,  a  vehicle  must  be  able  to  position  itself  with  only  minimal  contact  from 
any  external  systems  (i.e.  GPS).  Therefore,  the  AUV’s  Navigational  System  is  a  crucial 
subsystem  required  for  the  mainstream  utilization  of  AUVs.  However  a  Navigational 
Suite  must  not  only  be  accurate,  but  to  be  practical,  it  must  be  small  in  size,  a  low  power 
drain  on  the  vehicle,  and  low  cost. 

AUV  surface  navigation  can  be  accomplished  with  the  use  of  the  Differential 
Global  Positioning  System  (DGPS),  as  it  is  with  normal  surface  vessels.  DGPS  provides 
a  readily  available,  inexpensive  means  for  positioning  which  makes  it  very  suitable  for 
the  AUV.  However,  the  high  frequency  signal,  which  transmits  information  from  the 
GPS  satellites  to  the  AUV,  cannot  travel  through  the  water.  Therefore,  an  additional 
method  of  navigation  needs  to  be  incorporated  into  the  system  to  accommodate 
underwater  navigation. 

Manned  submersibles  in  deep  water  currently  use  inertial  navigation  while 
submerged.  However,  high  accuracy  Inertial  Navigation  Systems  (INS)  are  not  practical 


1 


for  many  AUVs  due  to  their  large  cost  and  size.  [Ref.  1]  Therefore,  while  the  vehicle  is 
submerged,  it  must  obtain  its  position  by  fusing  data  from  additional  internal  sensor 
measurements  (i.e.  speed,  and  heading).  The  increasing  cost  of  those  “additional” 
sensors  is  dependent  on  the  accuracy  they  produce.  By  limiting  the  sensors  with  a  “low 
cost”  objective,  the  system’s  ability  to  effectively  provide  the  AUV’s  position  becomes 
limited  by  the  amount  of  bias  error,  and  deviation  produced  by  cheaper  components. 
Commercially,  heading  sensors  could  range  from  a  Ring  Laser  Gyro  to  a  magnetic 
compass.  Similarly  for  speed,  velocity  sensors  vary  from  an  Acoustic  Doppler  Sonar  to  a 
simple  pitot  tube.  Accurate  Doppler  velocity  sensors  are  primarily  in  demand  because 
they  measure  the  vehicle’s  speed  relative  to  the  ocean  floor,  and  not  the  water  column 
itself.  Measurements  that  are  relative  to  the  water  are  susceptible  to  error  from  currents 
and  particulate  matter.  [Ref.  2]  Additionally,  “doppler  lock”  on  bottom  is  assured  for 
shallow  water  operations. 


B.  CURRENT  NAVIGATION  METHODS 

Several  methods  are  currently  employed  for  the  navigation  of  underwater 
vehicles,  the  most  basic  of  which  is  dead  reckoning.  Dead  reckoning  determines  the 
AUV’s  position  by  calculating  the  distance  traveled  from  its  measured  speed  and  time 
interval,  and  applying  it  in  the  direction  of  a  heading  measurement.  The  existing  errors 
present  in  the  velocity  measurements  grow  during  subsurface  periods  between  DGPS 
updates,  because  the  velocity,  with  an  error  bias,  must  be  integrated  to  determine 
position.  That  integration  causes  the  existing  error  to  become  larger. 
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A  second  method  of  navigation  uses  an  external  “base-line”  constructed  from 
signal  beacons  placed  in  the  water.  These  beacons  broadcast  a  range  signal  from  a 
known  position  to  allow  the  AUV  to  determine  its  own  position.  However  these  buoys 
require  moderate  to  high  levels  of  support  to  install  and  maintain,  depending  on  the  local 
environment.  Additional  support  for  this  method  can  consume  excess  time  and  money. 
An  ideal  system  uses  measurement  from  internal  sensors  to  determine  its  position 
underwater. 

Using  a  l°/hr  error.  Ring  Laser  Gyro,  Inertial  Motion  Unit  (IMU),  (such  as 
Honeywell’s  ‘HG  1700’  system,  c.  $12,000),  together  with  an  1%  speed  error,  “ground 
locked”,  Acoustic  Doppler  Sonar  (such  as  RDI’s  ‘Navigator  II’,  c.  $25,000),  leads  to  an 
expected  drift  error  of  1%  of  distance  traveled  for  lhr  of  travel  time.  In  other  words,  to 
limit  the  raw  error  to  2m,  with  a  speed  of  lm/s,  a  position  correction  fix  must  be  taken 
every  600- 1000m.  While  this  argument  is  not  precise,  it  indicates  that,  without  the  use  of 
acoustic  beacons,  DGPS  fixes  must  be  obtained  to  bound  the  error,  and  therefore  a 
combined  “IMU/Acoustic  Doppler/DGPS”  system  is  needed.  Its  sensor’s  measurements 
could  be  integrated  underwater,  and  cross-referenced  above  water,  to  accurately  estimate 
the  AUV’s  position  at  all  times. 

A  Kalman  filter  is  used  as  a  recursive  technique  for  integrating  sensor  data, 
velocity,  heading,  and  DGPS  position  when  it  is  available.  [Ref.  3] 

“The  Kalman  filter  performs  three  main  functions,  it  optimally  integrates  data 
from  multiple  sensors,  incorporates  models  of  the  sensors  error  characteristics,  and 
recursively  processes  the  measurements  from  the  sensors  that  are  available”.  [Ref.  2]  As 
a  byproduct  of  the  algorithms  that  compose  it,  the  filter  “learns”  biases  associated  with 
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the  heading  and  heading  rate  sensors.  It  is  its  ability  to  compensate  for  those  learned 
biases,  in  “Real-Time”,  which  makes  the  filter  a  valuable  component  for  an  AUV’s 
Navigational  Suite. 

C.  THESIS  SCOPE 

This  thesis  continues  the  work  of  several  previous  Masters  students  who 
researched  the  design  of  a  DGPS  system  for  the  Naval  Postgraduate  School’s  (NPS) 
AUV  program,  and  developed  a  Kalman  filter  program  for  use  in  AUVs.  [Ref.  5;  Ref.  4] 
The  objectives  of  this  report  are  to  properly  configure  the  designed  DGPS,  implement  it 
into  the  new  NPS  AUV,  the  ARIES  (Acoustic  Reconnaissance  Interactive  Exploratory 
Server),  and  experimentally  evaluate  it  in  an  open-water  environment.  Secondly,  adapt 
the  previously  developed  Kalman  Filter  software  for  use  in  the  ARIES.  And  finally, 
operate  that  adapted  program  in  conjunction  with  the  DGPS,  in  the  same  open  water 
environment,  and  obtain  positioning  results  with  near  sub-meter  accuracy. 

The  thesis  is  organized  in  the  following  manner;  Chapter  I  is  an  introductory 
chapter  which  provides  background  information  for  the  system  evaluation,  and  a 
description  of  the  main  objectives  for  the  report.  Chapter  II  provides  foundation 
information  about  DGPS,  and  the  key  points  addressed  during  the  configuration  of  a 
system  used  in  an  AUV.  Chapter  III  details  the  Navigational  Suite,  how  it  works,  and  its 
components.  Chapter  IV  provides  the  basic  theory  of  Extended  Kalman  Filtering  (EKF), 
and  introduces  the  concept  of  asynchronous  data  collection,  which  needed  to  be 
specifically  considered  during  the  filter’s  “real-time”  adaptation.  Chapter  V  discusses  the 
experimental  results  of  the  Navigation  Suite’s  evaluation  tests.  Finally,  Chapter  VI 
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presents  the  conclusions  of  those  results,  and  gives  some  recommendations  for  further 
research. 
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II.  GLOBAL  POSITIONING  SYSTEM 


A.  INTRODUCTION 

The  United  States  Global  Positioning  System  (GPS)  was  created  by  the 
Department  of  Defense  (DOD)  to  be  a  worldwide  resource  for  navigation.  GPS  satellites 
allow  for  radio  navigation  by  transmitting  range  and  time  information  to  receivers,  such 
as  in  the  ARIES  vehicle,  which  use  this  information  to  determine  the  vehicle’s  velocity, 
position,  and  time.  By  applying  publicly  broadcast  differential  corrections  to  these  coded 
radio  signals,  a  receiver  can  increase  the  accuracy  of  its  position  solution  to  within  5m. 
Naturally,  since  radio  waves  do  not  penetrate  through  saltwater  by  many  wavelengths, 
and  since  the  wavelengths  of  the  primary  GPS  signal  is  only  7.48in,  GPS  positioning  is 
essentially  confined  to  the  above  water  region  of  the  globe.  [Ref.  6] 

1.  GPS 

Dubbed  the  Navstar  system,  GPS  depends  on  a  constellation  of  24  active  and  3 
spare  satellites,  which  orbit  the  Earth  at  a  height  of  20,200km.  Each  satellite  orbits  the 
Earth  in  one  of  six  orbital  planes.  This  allows  for  5  to  9  satellite  to  be  visible  from  any 
point  on  the  earth  at  any  one  time.  [Ref.  7] 


GPS  Nominal  Constellation 
24  Satellites  in  6  Orbital  Planes 
4  Satellites  in  each  Plane 
20,200  km  Altitudes,  55  Degree  Inclination 

Figure  2.1:  GPS  Constellation.  [Ref.  8] 

GPS  satellites  broadcast  on  two  separate  radio  frequencies,  ‘LI’  (1575MHz) 
called  Coarse  Acquisition  Code  (C/A  Code),  for  general  public  Stand  Positioning  Service 
(SPS)  use,  and  ‘L2’  (1227MHz)  which  measures  ionospheric  signal  delay,  for  Military 
Precise  Positioning  Service  (PPS).  The  LI  code  is  intentionally  degraded  so  that  the 
global  public  does  not  have  access  to  precise  positioning  data,  but  still  allows  for  an 
average  position  accuracy  of  within  100m  on  the  horizontal  plane.  [Ref.  9] 

A  GPS  satellite  uses  the  LI  frequency  to  provide  a  receiver  with  the  encoded 
distance  measurement  between  the  receiver  and  that  particular  satellite,  referred  to  as 
‘Pseudorange’,  at  a  specific  time.  A  receiver  must  have  a  Pseudorange  from  at  least  3 
satellites  to  determine  a  2-dimensional  position,  and  at  least  4  satellites  for  a  3- 
dimensional  position,  using  spherical  triangulation.  The  fourth  satellite  acts  as  a 
crosscheck  of  the  calculation.  If  the  receiver  detects  more  than  4  satellites,  it  will  use  the 
satellites  with  the  4  best  elevation  angles  for  triangulation. 
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The  Global  Positioning  System 

Measurements  of  code-phase  arrival  tunes  from  at  least  four  satellites  are  used  to  estimate  four 
quantities:  position  in  three  dimensions  (X,  Y,  Z)  and  GPS  time  (T) . 


Figure  2.2:  GPS  Satellite  Triangulation.  [Ref.  8] 

The  natural  and  induced  degradation  of  Pseudorange  measurements  are  what 
determine  the  system’s  average  100m  horizontal,  156m  vertical,  and  340nsec  time 
accuracy.  [Ref.  8]  Ranging  errors  are  increased  by  differences  in  range  vectors  between 
the  satellites  and  receivers.  When  these  range  vectors  are  used  to  find  a  position  fix,  the 
volume  of  the  3 -dimensional  shape  described,  is  inversely  proportional  to  an  amount 
called  the  Geometric  Dilution  of  Precision  (GDOP). 

“DOP  is  a  mathematical  representation  of  the  quality  of  GPS  data  being  received 
from  satelites.”  [Ref.  10]  DOP  or  GDOP  is  often  broken  down  into  the  quantities  HDOP 
(horizontal),  VDOP  (vertical),  PDOP  (positional  or  spherical),  and  TDOP  (time).  Poor 
GDOP  is  represented  by  a  large  value.  This  means  that  the  geometric  volume  is  rather 
small,  and  the  satellites  have  inherently  poor  resolution  of  the  fix.  Good  GDOP  is  a 
smaller  number  reflecting  a  larger  geometric  volume,  and  good  triangulation.  Poor 
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GDOP  combined  with  signal  degradation  can  cause  a  GPS  accuracy  that  is  much  worse 
than  the  100m.  [Ref.  10]  Qualitatively,  DOP  is  a  statistical  probability,  between  1  and 
100,  of  the  amount  of  error  which  is  likely  to  be  present  in  relation  to  the  satellite  data 
used  to  determine  a  geographic  fix. 

“A  DOP  of  1  indicates  an  optimum  satellite  constellation,  and  high  quality  data.” 
For  data  to  be  useable,  it  is  generally  associated  with  a  PDOP  of  between  1  and  8.  [Ref. 
10] 


Figure  2.3:  Geometric  Dilution  of  Precision  (GDOP).  [Ref.  8] 

GPS  signal  degradation  is  due  the  effects  of  intentional  government  degradation 
called  Selective  Availability  (SA),  and  other  unintentional  errors  from  atmospheric  to 
computer  sources.  The  most  common  sources  of  individual  error  are: 

•  Selective  Availability 

•  Ionosphere 

•  Troposhere 

•  Satellite  Clock 

•  Code  Measurement 
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•  Receiver  Clock 


•  Multipath 

•  Satellite  Ephemerides 

All  of  these  errors  are  not  present  all  of  the  time.  However,  because  they  do 
severely  limit  the  receiver’s  positioning  accuracy,  most  receivers,  including  the  ARIES 
vehicle’s  receiver,  use  an  additional  second  measurement. 

Phase  measurements  are  carried  out,  by  the  receiver,  on  the  actual  encoded  signal 
itself.  The  receiver  measures  the  signal’s  phase  angle  is  observing  the  number  of 
wavelengths  and  partial  wavelengths  that  are  met  by  the  receiver’s  antennae  over  some 
period  of  time.  If  the  receiver  knows  the  exact  phase  angle  of  the  satellite  signal,  it  can 
correct  the  range  measurement  to  millimeter  accuracy.  [Ref.  9]  However,  a  single 
receiver  is  not  capable  of  determining  the  exact  signal  phase  angle. 

Phase  angle  measurements  also  contain  errors  due  to  the  error  sources  previously 
mentioned.  A  single  receiver,  because  of  these  errors,  cannot  accurately  determine  the 
number  of  wavelengths  between  the  satellite  and  itself.  This  means  that  the  accuracy 
obtainable  by  single-receiver  GPS  alone,  is  unsatisfactory  for  the  uses  of  maritime 
navigation. 

Therefore  in  1987,  the  U.  S.  Coast  Guard  (USCG)  began  work  on  a  method  for 
increasing  GPS  accuracy  to  bring  it  with  in  the  limits  set  forth  in  Federal  Radionavigation 
Plan  (FRP),  positioning  within  1  Om.  [Ref.  7]  This  lead  to  the  concept  of  broadcasting 
differential  corrections  from  a  permanent  reference  station. 
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2.  Differential  GPS 


Differential  GPS  (DGPS)  involves  the  use  of  a  reference  station  with  known  exact 
coordinates.  The  reference  station  receives  encoded  signals  from  the  individual  GPS 
satellites  in  view  from  its  position.  It  compares  the  pseudorange  measurement  from  each 
satellite  to  the  value  of  its  known  position,  and  derives  a  pseudorange  correction  (PRC) 
for  each  satellite’s  signal  independently.  These  corrections  are  then  broadcast,  in  a 
standard  format  by  both  public  and  private  parties,  for  the  use  of  GPS  receivers  in  the 
area. 


Figure  2.4:  DGPS  Position  Correction.  [Ref.  8] 


The  U.  S.  Coast  Guard  currently  provides  an  adequate  allotment  of  operational 
reference  stations  to  provide  differential  coverage  to  all  U.  S.  coastlines,  as  well  as  other 
major  U.S.  waterways.  These  reference  stations  broadcast  differential  corrections  for  the 
satellites  within  their  view  in  a  format  constructed  by  the  Radio  Technical  Commission 
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for  Maritime  Services  Special  Committee  104,  and  designated  RTCM  SC-104.  Not  all 
GPS  receivers  can  make  use  of  these  differential  corrections. 

An  adequately  equipped  receiver  contains  the  software  that  makes  it  capable  of 
applying  the  correction  provided  for  each  satellite  it  is  using,  to  determine  a  more 
accurate  position.  A  more  detailed  description  of  this  software  will  be  made  later,  with 
regards  to  the  ARIES  own  GPS  receiver.  In  addition,  such  a  remote  receiver  must  not 
only  have  a  connection  with  an  antennae  to  receive  satellite  signals,  but  also  an  antennae 
to  receive  the  corrections  broadcast  from  a  nearby  reference  station. 

Differential  corrections  are  most  useful  for  a  remote  receiver  that  is  close  to  the 
reference  station  because  the  they  are  only  corrective  of  the  first  5  errors  previously 
listed.  These  errors  are  only  common  to  both  a  reference  and  remote  station  if  their 
geographic  position  is  relatively  close,  within  100km,  because  they  are  largely 
atmospheric,  or  refer  to  the  same  satellites.  [Ref.  9]  The  other  2  errors,  multipath  and 
receiver  noise  cannot  be  corrected,  but  may  instead  be  reduced  by  a  technique  known  as 
‘Carrier  Phase  Smoothing’,  included  in  the  software  of  most  modem  receivers. 

When  the  7  major  errors  mentioned  are  accounted  for,  by  smoothing  or 
differential  corrections,  the  system  positioning  accuracy  is  on  average  of  l-5m.  [Ref.  9] 
Further  use  of  Carrier  Phase  Corrections  reduces  the  error  to  sub-meter  precision. 

B.  ARIES  DGPS  SYSTEM 

The  ARIES  DGPS  must  be  highly  effective  and  well  suited  for  the  vehicle. 
Specifically,  the  system  should  use  differential  corrections,  have  as  little  antenna  drag  as 
possible,  and  be  easily  adjustable  for  different  environmental  conditions.  Each  of  these 
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objectives  is  met  by  addressing  them  in  order  of  importance.  It  is  most  important  to 
ensure  accurate  position,  therefore,  it  is  most  important  to  ensure  the  vehicle  can  receive 
a  differential  signal.  Obtaining  differential  corrections  require  a  source  for  those 
corrections,  and  a  beacon  that  picks  up  that  signal. 

1.  System  Constraints 

The  Monterey  Bay  Aquarium  Institute  (MBARI)  broadcasts  a  differential  signal 
at  466.76MHz.  It’s  perpetuated  from  a  Mount  Toro  repeater  and  covers  the  entire  Bay 
area.  [Ref.  5]  However  this  signal  suffers  frequent  periods  of  outages,  which  make  it 
unsuitable  for  the  continuous  navigation  of  an  AUV,  powered  by  time-limited  batteries. 

As  previously  mentioned,  the  USCG  offers  publicly  broadcast  differential 
corrections  to  all  major  U.  S.  coastlines  and  waterways.  The  Monterey  Bay  source  of 
these  corrections  is  located  north  of  Santa-Cruz  at  Pigeon  Point,  and  broadcasts  its  signal 
at  287kHz.  The  other  broadcast  stations  broadcast  on  a  frequency  range  of  283.5  - 
325kHz.  [Ref.  5] 

This  introduces  the  first  system  constraint.  Differential  signal  beacons  are 
manufactured  to  receive  a  particular  signal  frequency.  This  would  be  fine  if  the  ARIES 
vehicle  stayed  within  the  broadcast  range  of  the  Pigeon  Point  station.  However,  NPS 
Center  for  AUV  Research  currently  participates  in  various  exercises  across  the  globe. 
Some  of  these  exercises  include  annual  participation  in  ‘AUVfest’  located  offshore  of 
Gulfport,  MS.,  and  a  planned  cooperative  exercise  in  the  Azores,  with  the  University  of 
Lisbon.  Obtaining  beacons  constructed  to  receive  signals  in  each  of  these  areas,  as  well 
as  removing  and  installing  these  beacons  with  each  mission,  is  neither  cost  effective  or 
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practical.  A  system  is  needed  which  can  be  altered  to  accept  a  range  of  frequencies.  A 
second  system  constraint  centers  about  the  ARIES  antennae. 

The  vehicle  requires  an  antenna  for  the  communication  of  the  command/control 
signal  from  a  remote  base-station.  This  allows  for  the  operators  to  view  files  collected  by 
the  AUV  in  real-time,  as  well  as  the  communication  of  new  commands  or  missions  to  the 
vehicle.  A  second  antenna  will  be  required  by  the  GPS  receiver  to  collect  satellite  data 
and  establish  a  position  fix.  Finally,  because  the  differential  corrections  signal’s 
frequency  (kHz)  is  so  much  lower  than  the  satellite’s  C/A  frequency  (MHz),  the 
differential  signal  requires  a  much  larger  antennae. 

These  varying  system  constraints  require  separate  antennae,  which  will  in  turn 
increase  vehicle  drag  to  an  unacceptable  level.  A  solution  must  be  created  which  can 
reduce  the  size  or  number  of  objects  attached  to  the  outside  of  the  vehicle  hull. 

2.  System  Solution 

The  problem  of  varying  geographically  varying  signal  frequency  was  solved  with 
the  use  of  a  differential  receiver.  This  receiver  is  capable  of  varying  the  signal  it  excepts, 
based  on  the  signal  being  picked  up  by  the  beacon.  One  the  corrections  are  accepted, 
they  are  passed  along  to  the  GPS  receiver,  which  uses  them  with  to  correct  the  satellite 
data  it  collects,  and  determines  an  accurate  fix.  However,  the  addition  of  a  differential 
receiver  requires  more  space,  which  is  limited  within  the  AUV  hull.  This  brings  about 
discussion  of  a  solution  to  the  drag  problem  due  to  too  many  antennae. 

The  current  vehicle  base-station  was  used  to  locate  the  differential  receiver  and 
beacon.  This  allows  for  the  differential  beacon  to  be  sized  however  necessary  to  receive 
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any  frequency  differential  signal,  and  not  hinder  the  fluid  flow  over  the  vehicle.  These 
corrections  will  be  sent  to  the  vehicle  over  a  900Hz  radio  signal  through  a  radio  modem, 
and  received  through  the  command/control  aerial  by  a  splitter  connection  to  a  second 
radio  modem.  The  splitter,  called  a  “Multiplexer”,  makes  it  possible  to  receive  the 
corrections  for  the  radio  modem,  as  well  as,  the  command  signals  for  the  tactical 
computer,  on  the  same  antenna.  More  detailed  descriptions  of  the  vehicle’s  hardware  is 
presented  in  Chapter  3. 
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III.  ARIES  NAVIGATIONAL  SUITE 


A.  INTRODUCTION 

The  ARIES  hardware  was  selected  to  meet  the  three  basic  objectives  of  size, 
accuracy,  and  cost.  The  component’s  size  is  important  because  it  effects  the  size  of  the 
drag  forces,  which  act  on  the  vehicle  during  underwater  transit.  If  a  sensor  is  externally 
mounted  to  the  vehicle,  it  adds  a  drag  force  with  the  addition  of  it  own  profile.  If  the 
component  is  an  internal  component,  it  adds  weight  to  the  vehicle,  which  must  be 
compensated  for  by  increasing  the  size  of  the  vehicles  wetted-surface  area  to  maintain 
proper  ballast,  which  will  again  effect  the  drag.  The  overall  accuracy  of  the  Suite  is 
dependent  upon  the  sum  of  the  accuracy  of  individual  component.  Therefore  the 
selection  of  the  components  used  in  the  ARIES  AUV  was  based  on  a  balance  between 
their  cost,  and  the  degree  of  accuracy  they  provide.  Their  combined  accuracy  is  expected 
to  produce  a  navigational  error  of  less  than  one  meter  in  the  final  operating  configuration. 

1.  Navigational  Suite  Overview 

The  ARIES  navigational  suite  only  represents  a  part  of  ARIES  entire  computer 
architecture.  The  following  diagram  of  the  vehicle’s  internal  systems  shows  the 
interaction  of  the  navigational  system  (green)  with  regards  to  all  other  components. 
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Figure  3.1:  ARIES  AUV  Computer  Architecture.  [Ref.  11] 


The  Navigational  Suite  is  designed  to  provide  the  ARIES  CPU  with  a  position 
based  on  information  from  4  sensors.  In  addition  to  the  DGPS  system,  a  bottom-mounted 
Doppler  Sonar  Velocity  Log,  internally  mounted  Inertial  Motion  Unit  (IMU),  and 
internally  mounted  depth  cell,  also  provide  basic  information  for  the  use  in  constructing 
an  accurate  map  of  the  vehicle’s  path  by  the  vehicles  computers.  The  interaction  of  these 
navigational  components,  within  the  navigation  suite,  can  be  viewed  in  the  diagram 


below. 


B.  SYSTEM  COMPONENTS 

The  complete  Navigational  Suite  consists  of  the  following  components: 

•  GPS  Receiver  and  Power  Supply 

•  Differential  Corrections  Receiver  and  Power  Supply 

•  GPS  Antennae 

•  Differential  Beacon 

•  2  Radio  Modems  and  Power  Supply 

•  Base-station  Radio  Antennae 

•  Command/Control  Antennae 

•  Doppler  Sonar  Velocity  Log 

•  Inertial  Motion  Unit  (IMU) 

•  Depth  Cell 

•  ARIES  Navigational  Filter 
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To  better  understand  these  how  these  components  interact,  it  is  important  to  know  the 
specifics  on  each  piece  of  hardware  and  software  used  within  the  suite. 

1.  Base-Station  Hardware 

As  mentioned  earlier  in  this  study,  there  are  two  separate  parts  of  the  DGPS.  The 
base-station  components  consist  of  the  differential  receiver,  the  differential  beacon,  and 
the  radio  modem.  The  differential  receiver  and  beacon  are  connected  to  the  radio  modem 
using  an  RS  232  serial  port  connection.  Both  the  receiver  and  modem  can  be  internally 
configured  by  connecting  them,  using  the  same  serial  ports,  to  a  computer  with  the 
appropriate  software.  This  software  will  be  discussed  following  the  DGPS  hardware 
section  of  this  study. 


Figure  3.3:  DGPS  Base-Station  Configuration. 


a.  The  Differential  Receiver  and  Beacon 

The  differential  receiver  and  beacon  used  with  the  base-station  are 
products  of  Communication  System  International  (CSI),  and  called  the  ‘ABX-3’  receiver, 
and  ‘MBL-3’  magnetic  field  antenna.  The  ABX-3  receiver  requires  a  9-40V  DC  external 
power  source,  and  the  MBL-3  receiver’s  power  from  its  cable  connection  to  the  ABX-3. 
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Both  products,  as  seen  below,  operate  with  specific  features,  which  make  them  valuable 
for  the  ARIES  project. 


Figure  3.4:  Differential  Receiver  and  Beacon  [Ref.  5]. 

The  main  feature  of  the  ABX-3  receiver  is  its  ability  to  work  under  a 
manual  tuning  or  automatic  tuning  mode.  The  manual  tuning  mode  is  activated  using  a 
standard  National  Marine  Electronics  Association  (NMEA)  0183  format  for  commands 
and  queries.  This  format  can  be  seen  in  Appendix  A  as  it  appears  in  Ref.  5.  The  ABX-3 
can  be  automatically  tuned  to  any  frequency  within  the  USCG  frequency  range  (283.5  - 
350kHz),  using  an  option  called  Automatic  Beacon  Search  (ABS)  [Ref.  9] 

The  ABS  works  by  using  two  separate  operating  channels.  Each  channel 
searches  for  a  separate  broadcast  frequency  in  the  RTCM  SC-104  format.  Once  a 
channel  locates  a  signal,  it  locks  onto  that  signal,  evident  by  a  green  light  on  the  front  of 
the  ABX-3,  and  relays  those  corrections  over  the  RS  232  cable.  Meanwhile  if  the  other 
channel  locates  a  frequency,  which  exhibits  a  signal  strength  (SS)  2dB  greater  than  the 
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current  locked  signal,  the  receiver  will  automatically  switch  to  this  signal,  lock  onto  it, 
and  begin  relaying  those  corrections  to  the  serial  cable.  [Ref.  5]  This  is  ideal  for  a 
situation  in  which  the  base-station  may  be  mobile,  and  drifting  between  two  USCG 
broadcast  areas  of  different  signal  frequencies. 

The  main  feature  of  the  MBL-3  beacon  is  its  use  of  an  H-field  type  loop 
antenna.  The  H-field  loop  is  less  susceptible  to  noise  than  a  tradition  antenna,  does  not 
require  a  ground  connection,  and  takes  up  less  physical  space  than  a  traditional  antenna. 
[Ref.  9] 


b.  The  Radio  Modem 

The  radio  modem  used  to  connect  the  base-station  to  the  vehicle  is  a 
product  of  Freewave  Technologies  Inc.,  and  known  simply  as  the  ‘Freewave  Modem.’ 
This  modem  always  works  with  an  identical  modem,  can  be  configured  using  software, 
and  requires  a  12  or  24V  DC  external  power  source.  For  its  use  with  the  ARIES  DGPS, 
the  standard  master  (base-station  modem)  to  slave  (AUV  modem)  configuration  has  been 
set.  The  modems  operate  at  an  alterable  baud  rate  of  9600bps.  The  modems  are  also 
configured  for  specific  channels  of  communication,  once  any  point  to  point  mode  (ex. 
Master  to  Slave)  is  selected.  The  Freewave  modem  can  be  seen  in  Figure  3.5  below. 
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Figure  3.5:  A  Freewave  Radio  Modem  Pair.  [Ref.  5] 


2.  AUV  Hardware 

The  AUV  internal  navigation  components  consists  of  the  GPS  receiver,  the  slave 
radio  modem,  the  GPS  aerial,  the  command/control  antenna,  a  slave  Freewave  modem 
connected  to  a  splitter  from  the  antenna,  the  Acoustic  Doppler,  IMU,  and  depth  cell.  The 
splitter  connection,  called  a  “Mulitplexer,”  differentiates  between  the  combined 
differential  corrections  and  command/control  signals  received  by  the  two-way 
command/control  antenna.  The  AUV’s  internal  batteries  power  all  of  the  internal 
components,  which  require  a  power  DC  source.  A  60V  DC  bus  carries  DC  power  to  the 
internal  compartments  of  the  vehicle,  while  DC-DC  converters  are  used  to  provide  5,  12, 
24,  and  48V  DC  power  sources  for  subsystems. 

a.  The  GPS  Receiver  and  Aerial 

The  DGPS  portion  on  this  end  of  the  modem  connection  receive  the 
corrections  and  send  them  to  the  GPS  receiver  via  an  RS  232  serial  cable,  as  seen  below 
in  Figure  3.6. 
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Figure  3.6:  DGPS  AUV  Configuration. 


The  GPS  receiver  is  an  Ashtech  ‘G-12’  model.  It  is  connected  to  the  GPS 
aerial  by  a  direct  cable,  which  passes  through  a  sealed  hull  penetration.  This  is  necessary 
to  reduce  the  number  of  connections  between  the  aerial  and  the  receiver,  thus  reducing 
any  possible  problem  which  may  occur  due  to  impedance  mismatches  and/or  lack  of 
signal  strength  from  the  satellites. 

The  G-12  is  capable  of  tracking  up  to  12  satellites  at  one  time,  however  it 
only  uses  the  best  4  signals  to  calculate  a  fix.  The  number  of  satellites  being  tracked  by 
the  receiver  can  be  readily  observed  by  counting  the  green  flashes  given  off  between  red 
flashes  on  its  external  LED.  When  the  receiver  is  tracking  at  least  4  satellites  and  has  a 
PDOP  less  than  4,  it  generally  returns  a  position  with  an  error  less  than  16m.  A  complete 
readout  of  the  satellite  information  can  be  viewed  by  connecting  the  G-12  to  a  computer 
via  a  serial  connection,  and  reading  the  positioning  information  with  the  custom  software 
provided,  which  is  described  later.  [Ref.  12]  The  G-12  can  be  seen  below  in  Figure  3.7. 
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[Ref.  12] 

The  GPS  aerial  is  a  MicroPulse  model  ‘18800’,  internally  amplified,  26dB 
aerial.  It  needs  be  mounted  only  6”  above  the  water  surface  to  receive  satellite  data,  and 
works  best  when  it  is  mounted  on  top  of  a  2x2in  metal  ground  plate.  Because  the  aerial  is 
itself  only  1.5  x  1.5  x  0.5in,  it  creates  little  extra  drag  for  the  vehicle.  It  is  constructed  of 
a  polymer  resin  and  sealed  to  prevent  water  damage.  [Ref.  13]  The  18800  can  be  seen  in 
Figure  3.8  below,  as  it  is  mounted  on  the  ARIES.  The  picture  shows  the  aerial  mounted 
to  a  ground  plate  (white)  on  top  of  the  rear  rudder  guard  (white).  The  “aerial  to  G-12” 
cable  passes  through  a  hull  penetration  immediately  behind  the  mdder  (blue). 
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Figure  3.8:  Mounted  GPS  Aerial  and  Cable  Hull  Penetration. 
b.  Command/Control  Antenna  and  Multiplexer 

The  command/control  antenna  and  combination/splitter  connection, 
referred  to  as  a  “Multiplexer”,  are  also  both  new  to  additions  for  the  ARIES  DGPS.  The 
antenna  currently  used  on  the  ARIES  is  a  12”  long,  0.5”  diameter,  rigid  antenna  designed 
and  manufactured  by  Webb  Research  Institute.  This  antenna  receives  signals  from  both 
the  command/control  and  differential  corrections  radio  modems.  It  splits  the  two  signals 
using  an  internally  mounted  75ohm,  signal  splitter,  which  then  sends  each  signal  to  their 
respective  slave  modems  located  in  the  AUV.  [Ref.  6]  The  antenna  is  capable  of 
transmitting  and  receiving,  and  is  the  vehicle’s  primary  method  of  wireless 
communication.  It  is  mounted  on  top  of  the  ARIES,  2/3  of  the  way  along  the  body  from 
the  front,  and  shown  in  Figure  3.9. 
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Figure  3.9:  Two-way  Command/Control  and  Differential  Corrections  Antenna, 
c.  Sonar  Doppler  Velocity  Log 

The  Sonar  Doppler  Velocity  Log  is  an  “Workhorse  Navigator”  1200  kHz 
model,  manufactured  by  RD  Instruments.  It  measures  the  vehicles  forward  and  lateral 
speed  referenced  to  the  bottom  or  water  column,  as  well  as  the  vehicle’s  altitude,  pitch, 
roll,  and  heading.  The  device  uses  4  convex  transducers  aimed  30  degrees  from  vertical 
to  send  out  active  pings  at  a  frequency  of  2  Hz.  The  Doppler  is  rated  for  a  depth  of 
2000m  and  is  powered  by  a  24V  DC  source  from  within  the  AUV.  The  range  for  ground 
lock  for  this  unit  is  30m,  although  this  value  could  be  increased  with  the  use  of  a  lower 
frequency  ping.  For  instance,  the  600kHz  unit  has  a  maximum  altitude  of  90m.  [Ref.  14] 
It  is  shown  in  Figure  3.10  below. 
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Figure  3.10:  RDI  Workhorse  Navigator  Doppler  Velocity  Log.  [Ref.  14] 


d.  IMU 

The  Inertial  Motion  Unit  is  manufactured  by  Systron  Donner,  and 
internally  mounted  on  the  AUV.  The  ‘IMU’  is  a  “solid-state”  6  degree  of  freedom 
inertial  sensor  which  measures  angular  rates  and  linear  accelerations  with  3  quartz 
angular  rate  sensors,  and  3  linear  servo  accelerometers.  It  is  powered  by  15  V  DC  from 
within  the  AUV,  and  produces  analog  measurements.  A  separate  process  reads  digitized 
IMU  data  with  a  16-bit  A/D  at  100Hz,  and  sub-samples  that  data  for  use  at  the  control 
rate  of  8Hz.  [Ref.  1 5]  It  is  shown  in  Figure  3.11. 
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Figure  3.11:  Systron  Donner  IMU.  [Ref.  15] 
e.  Depth  Cell 

The  depth  cell  used  in  the  ARIES  is  a  pressure  transducer/transmitter 
manufactured  by  PSI  Tronix  and  is  model  ‘PWC’.  It  has  a  depth  range  of  0-15psi 
absolute,  and  operates  by  measuring  the  strain  on  an  internal  diaphragm  with  Silicon 
strain  gages.  [Ref.  16]  It  is  powered  by  the  vehicle’s  batteries,  and  is  shown  in  Figure 
3.12. 


Figure  3.12:  PSI  Tronix  PWC  Tressure  Transducer/Transmitter.  [Ref.  16] 
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3.  System  Software 

The  ARIES  navigational  suite  operates  with  several  pieces  of  equipment  that  need 
to  be  manually  configured,  as  previously  mentioned,  on  a  computer.  These  components, 
namely  the  ABX-3,  the  Freewave  Modems,  and  the  G-12,  are  connected  to  a  computer 
through  the  computer’s  COM-1  port  with  a  serial  cable.  Once  connected,  each 
component  can  be  configured  if  its  output  is  read  with  the  appropriate  software,  set  to  the 
appropriate  settings.  There  are  two  main  types  of  software  used  to  read  these  outputs, 
one  for  the  ABX-3  and  radio  modems,  and  one  for  the  G-12. 

a.  Procomm  Plus  ( Radio  Modem ) 

‘Procomm  Plus’  is  a  program  offered  by  Quaterdeck.  It  can  be  configured 
for  terminal  emulation  of  36  different  terminals,  and  file  transfer  with  11  different 
protocols.  It  is  used  to  display  the  simple  terminal  emulation  that  comes  from  the 
modems  setup  menu.  Procomm  Plus  runs  on  Windows  platforms  and  may  be  customized 
to  suite  the  needs  of  the  user.  The  software  was  specifically  used  with  the  hardware  to  set 
the  modem  operating  mode  and  the  data  transfer  baud  rate.  [Ref.  17] 

b.  Command  Message  Software  ( G-12  and  ABX-3 ) 

This  software  is  a  program  that  relays  commands  and  queries  to  the  G-12 
and  ABX-3  through  a  serial  connection.  It  also  allows  the  user  to  view  the  current 
configuration  of  the  receiver,  and  change  any  options  that  you  choose.  That 
configuration  change  may  be  as  simple  as  to  reset  the  time  between  output  fixes,  the 
communication  baud  rate,  or  as  complex  as  changing  the  setup  to  recognize  the 
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differential  corrections  coming  from  a  remote  source,  as  it  is  with  the  ARIES.  A  list  of 
example  software  commands  and  queries  frequently  used  in  the  ARIES  AUV  in  relation 
to  the  G-12  and  ABX-3,  and  their  meanings,  may  be  found  below.  [Ref.  12] 

•  $PASHS,NME,POS,A,ON  -  Enables  a  display  of  the  NMEA  position 
message  on  port  A  (G-12). 

•  $PAS HS ,RT C,REM,B ,ON  -  Sets  Port  B  receiver  to  operate  as  the 
differential  remote  station,  and  enables  it  (ABX-3). 

•  $PASHS,NME,SAT,A,ON  -  Enables  a  display  of  the  NMEA  satellite 
information  message  on  port  A  (G-12). 

•  $PASHQ,PAR,x  (where  x  is  A  or  B)  -  Queries  the  current  settings  of 
the  specified  port  (G-12  and  ABX-3). 

•  $PASHQ,RTC,B  -  Queries  the  current  differential  settings  of  port  B 
(ABX-3). 

•  $PASHS,SAV,x  (where  x  is  Y  or  N)  -  Enables  or  disables  automatic 
saving  of  set  parameters  in  the  battery  backed  up  memory  (G-12  and 
ABX-3). 

•  $PASHQ,SAT,A  -  Queries  the  status  of  the  SV’s  currently  locked  by 
the  receiver  on  port  A  (G-12). 
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IV.  KALMAN  FILTERING 


A.  INTRODUCTION 

A  Kalman  filter  is  a  programmed  process  that  provides  an  optimal  estimate  of 
state  variables  with  a  least  squares  error,  given  a  set  of  measurement  data  and  a  model. 
This  estimate  is  based  on  measurements  retrieved  from  sensors,  information  about  those 
sensors,  and  prior  information  about  the  system.  [Ref.  18]  Key  references  for  the 
theoretical  development  of  Kalman  Filters  are  Kalman,  1960,  Gelb,  1989,  and  Bar- 
Shalom,  1993.  The  filter’s  application  to  low  cost  AUV  navigation  has  been  outlined  in 
Healey,  An,  and  Marco,  1998. 

B.  KALMAN  FILTER  THEORY 

Although  Kalman  filters  can  be  discrete  or  continuous,  the  ARIES  vehicle  uses  a 
digital  computer,  which  is  only  capable  of  discrete  time  interval  computation.  However, 
the  model  represented  by  the  vehicle  dynamics,  is  a  continuous  state  model,  therefore  a 
discrete  time,  continuous  state  filter  is  needed  for  vehicle  navigation.  To  accomplish  this 
first  requires  an  understanding  of  how  a  discrete  filter  works. 

1.  Discrete  Kalman  Algorithms 

In  the  ARIES  AUV  data  does  not  continuously  flow  from  the  sensors.  Instead, 
each  sensor  has  a  specified  nominal  measurement  rate,  at  which  it  delivers  that 
measurement  to  the  navigation  process. 

Five  major  algorithms  for  Kalman  filters  represent  the  discrete  time  measurement 
and  the  recursive  estimation  of  both  the  model  state,  and  its  error  covariance.  In  a  multi- 
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variable  system,  such  as  the  ARIES  navigational  filter,  the  system  and  measurement 
models  are  represented  by  the  equations: 

xm  =<Pxu  +qk 

yk=Cxk+vk  (1) 

In  these  equations,  k  is  the  time  step,  x  is  the  system  state  and  x  e  318j:I  ,  y  is  the 
measurement  and  y  €  ,  C  is  the  matrix  relating  output  to  the  state  variable,  and  q  and 

v  are  assumed  to  be  gaussian  white  noise,  with  zero  means  and  designated  covariance.  & 
is  the  state  transition  matrix,  which  has  its  characteristics  from  the  continuous  state  model 

dynamics  matrix,  A,  for  this  system  (<p  =  eAT ,  where  T  is  the  update  time).  [Ref.  19] 

To  begin  the  recursive  process  of  running  the  algorithms  for  each  time  step,  each 
of  the  variables  used  in  these  algorithms  must  be  initialized  to  some  value.  When  the 
filter  is  started  for  the  first  time,  this  value  will  be  determined  by  the  programmer,  each 
successive  iteration  of  the  algorithm  will  define  the  variables  as  the  last  estimated  value. 

The  first  major  algorithm,  equation  (2)  shows  the  propagation  of  the  state  by 
using  the  model  (1)  expressed  as  a  mean  value. 

/*=***/*;  (2) 
This  equation  shows  xk+]/k ,  which  is  the  estimated  state  at  time  step  k+1,  based  on  the 
estimated  state  at  time  step  k.  The  filter  actually  estimates  the  state  twice  using  the 
algorithm  in  a  prediction/correction  mode.  This  first  estimate  is  based  solely  from  the 
state  model.  The  second  estimate  will  be  based  on  the  fusion  of  the  first  estimate  and  the 
data  from  the  measurement. 
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The  second  major  algorithm,  equation  (3),  estimates  the  error  covariance  for  the 
k+1  time  step,  based  on  the  error  covariance  from  time  step  k,  and  the  additive  designated 
system  uncertainty  matrix  Q,  derived  from  the  system  noise,  q. 

^*k+\lk  ~  9^*klk9  +  Q'n  (3) 

The  error  covariance  will  also  be  estimated  twice  during  the  complete  filter  loop.  The 
second  estimate  will  be  made  from  the  correction  estimate  of  the  state  variables.  Before 
those  estimates  can  be  made,  however,  the  filter  gain  must  be  determined,  which  will 
minimize  Pk+Uk+l . 

The  filter  gain  is  calculated  in  the  third  major  algorithm,  equation  (4).  This  gain 
is  determined  based  on  the  newly  estimated  error  covariance  as  it  relates  to  the  output 
variables,  as  well  as  the  designated  measurement  channel  noise. 

G  =  Pk+UkCT(CPk+UkCT+R)-';  (4) 

In  this  equation  G  is  the  filter  gain,  and  R  is  the  matrix  of  measurement  channel 
designated  noise  covariance.  This  gain  is  used  for  the  correction  of  the  state  variables. 

Equation  (5)  below  shows  the  next  major  algorithm.  Here  the  state  variables  are 
estimated,  by  adding  the  prediction  with  a  difference  between  the  new  measurement  and 
the  value  predicted  by  first  estimate,  and  weighted  by  the  filter  gain. 

*k+Vk+\  =  *k+\lk  +^(T*+1  ~CXk+Uk)’  (5) 

In  this  equation,  xk+uk+l  is  the  correction  estimate,  at  the  time  step  k+1,  based  on  the 
measurement  taken  at  time  k+1. 

A+l/i+l  =  I*k+\lk  GCPk+m ;  (6) 
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In  equation  (6),  the  final  major  algorithm,  the  second  error  estimate  is  determined 


for  the  k+1  time  step.  Pk+]/k+i  is  always  less  than  Pk+Uk  since  GCPk+Uk  is  a  positive 
value.  [Ref.  19] 

After  the  filter  has  determined  the  corrected  estimates  of  both  error  covariance 
and  the  state  variables,  it  will  then  go  back  to  equation  (1)  and  (2)  and  define  an  initial 
estimate  for  the  next  time  step,  and  continue.  Each  time  the  filter  will  provide  state 
estimates  with  the  minimal  (optimal)  error  covariance,  and  continually  reducing 
covariance  estimates.  This  system  works  well  for  specific  discrete  time  data.  But  as 
previously  mentioned,  the  ARIES  control  systems  are  modeled  after  a  continuous  state 
model.  Therefore  understanding  the  difference  between  the  two  models  is  crucial  to 
understanding  the  ARIES  Navigation  Filter. 

2.  Continuous  State  Model 

In  a  continuous  time  model,  both  “the  system  model  and  the  output  equation  are 
nonlinear.”  [Ref.  3] 

y(t)  =  h(x(t))  +  v(ty,  () 

x(t)  €  SR®*7  is  the  model  state,  q  and  v  are  again  noise  to  the  system,  and  /  and  h  are 
continuous  functions  differentiable  by  x(t). 

For  a  locally  linearized  model, 

x(t)  =  Ax(t)  +  q; 

y(t)  =  Cx(t)  +  v;  (8) 

where  A  -  —  and  C  =  — ; 

dx  dx 
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For  the  ARIES,  the  states  are  the  globally  referenced  longitude  and  latitude  in  meters,  X 
and  Y,  the  heading  angle  referenced  to  North,  'F,  the  vehicle’s  yaw  rate  from  a  rate  gyro, 
r,  the  body  referenced  forward  and  lateral  speed  over  ground,  ug  and  vg,  and  a  bias  on  the 
sensor  readings  from  the  rate  gyro  and  compass  heading,  br  and  by.  These  values  are  all 
present  in  the  system  state  vector,  x. 

x  =  [x,Y,y/,ug,vg,r,br,bl//  J  (9) 

These  values  are  each  present  in  a  nonlinear  equation  which  must  be  solved  to 
find  their  true  values.  These  equations  are  represented  in  matrix  form  as  the  A  matrix, 
which  will  be  discussed  in  detail  later  in  this  chapter.  However,  the  equations,  seen 
below,  represent  how  each  state  variable  is  used  to  dynamically  determine  each  of  the 
others. 

X  =  ug  cos(y/)~  vg  sin(^}, 

Y  =  ug  sin(^)+  vg  cos(^); 

V  =  r; 

u  =0; 

*.-0=  (1#) 

r  =  0; 

br  =  0; 

K  =  0; 

To  solve  for  the  equations  and  determine  a  solution  for  the  state  variables,  the  control 
system  relies  on  the  ARIES  navigational  sensors  for  measurements.  The  sensors  provide 
data  for  the  each  of  the  states  except  the  biases,  which  the  filter  is  told  to  be  naturally 
included  in  the  measurement.  The  measurements  enter  the  filter  in  the  matrix  form  of  a 
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matrix,  Q,  and  the  estimated  states  are  converted,  using  a  conditionally  specified  matrix, 
C,  into  the  output  matrix y.  This  matrix  is  in  the  form: 

y  =  [ug,vg,y,X,Y]\  (11) 

The  equations,  which  are  related  in  the  C  matrix  and  determine  the  output  based  on  what 
the  system  is  receiving  from  the  sensors,  are  as  follows: 


y,=ug-, 

(12) 

y*=r  +  br; 

y5=x-, 

y6  =  Y; 

This  output  is  then  used  to  determine  the  vehicle’s  position  based  on  three 
separate  methods.  The  first  method,  longitude  and  latitude,  is  a  simple  plot  of  the  DGPS 
coordinates,  which  have  been  converted  to  meters,  and  referenced  to  the  initial  position. 
The  second  method  uses  traditional  dead  reckoning,  in  which  the  heading  and  speed  are 
simply  used  to  plot  the  vehicle’s  ground  track.  The  final  method  for  determining  vehicle 
position  uses  a  combination  of  method  one  and  two,  and  attempts  to  determine  the  best 
possible  answer,  using  the  filter  solution.  This  method  will  be  more  closely  examined  in 
the  discussion  on  the  ARIES  Navigational  Filter. 


C.  ARIES  NAVIGATIONAL  FILTER 

The  ARIES  Navigational  Filter  is  a  crucial  piece  of  the  vehicle’s  software.  It 
allows  the  vehicle  to  accurately  navigate  in  and  around  the  areas  necessary  to  perform  its 
missions,  perform  those  missions  with  greater  effectiveness  by  globally  referencing  its 
operational  area  and  objects  in  that  area,  and  finally,  makes  way  for  future  progress  in  the 
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field  of  discovering  new  vehicle  applications.  All  of  these  capabilities  are  enabled 
through  accurate,  low  cost,  vehicle  navigation.  An  explanation  of  the  vehicle’s  filter 
code,  which  can  be  seen  as  Appendix  B,  will  describe  how  this  is  accomplished. 

As  previously  mentioned  the  ARIES  filter  takes  measurements  from  its  sensors, 
and  then  estimates  its  position.  These  measurements,  namely  the  Doppler  forward  and 
lateral  speed,  the  IMU  yaw  rate,  and  the  differentially  corrected  longitude  and  latitude, 
must  be  formatted,  or  pre-processed,  to  be  correctly  displayed  and  used  in  the  correct 
units. 


1.  Data  Pre-Processing 

The  longitude  and  latitude  are  both  presented  to  the  filter  in  the  form  of  degrees 
and  decimals  of  degrees.  The  filter  code  changes  both  of  these  measurements  into 
meters.  Additionally,  to  be  correctly  plotted,  the  filter  references  the  entire  latitude  and 
longitude  data  set  to  the  origin  of  the  data  (first  data  point).  The  heading  measurement 
also  requires  correcting. 

The  heading  measurement  taken  by  the  RDI  compass  is  displayed  as  a  positive  or 
negative  number  of  degrees,  depending  on  which  side  of  the  North/South  line  the  AUV  is 
pointed.  However,  the  filter  requires  continuous  data  without  the  attendant  360° 
discontinuity.  Therefore,  the  heading  is  changed  using  a  “Wrap-Count”  method  that 
allows  the  vehicle’s  heading  to  continue  to  grow  beyond  360°  if  necessary,  keeping 
account  of  the  number  of  turns  completed  by  the  vehicle  as  it  “winds  up”  and  “unwinds” 
during  turns. 
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For  example,  for  one  complete  counterclockwise  rotation,  the  old  data  format 
would  show  a  plot  from  0°  to  180°,  then  an  instantaneous  360°  jump  to  -180°,  and  finally 
a  plot  from  -180°  up  to  0°  again.  The  new  format  will  simply  show  a  continuous  plot 
from  0°  to  360°.  If  the  vehicle  made  two  complete  rotations,  the  plot  would  go  from  0°  to 
720°,  and  so  forth.  Finally,  after  the  format  is  changed,  the  heading  is  converted  to 
radians.  Once  these  corrections  have  been  made,  the  filter  is  initialized. 

As  part  of  initialization,  the  filter  defines  the  governing  control  matrices,  and  the 
time  base  it  using  to  run  recursively  operate  the  loop.  This  time  base  is  determined  from 
the  speed  of  the  sensor  that  takes  a  measurement  most  frequently.  After  that  the  filter 
uses  its  previous  knowledge  of  the  system  model  to  prepare  the  state  dynamic  matrix  A, 
and  inputs  given  bias  and  system  noise  matrices  Q  and  R,  it  defines  the  output  matrix  C 
based  on  the  measurement  data.  This  can  be  better  explained  with  a  detailed  look  at  each 
matrix. 


2.  A  Matrix 

The  A  matrix  for  the  ARIES  AUV  is  an  8  by  8  matrix  defined  using  the  known 
equations  for  underwater  marine  vehicle  dynamics  given  as  equation  (10).  These 
equations  contain  trigonometric  relationships,  which  are  the  source  of  the  system  non¬ 
linearity.  However  those  relationships  are  necessary  to  combine  both  globally  and  body 
referenced  information.  [Ref.  3]  From  the  A  matrix  and  the  time  base,  the  filter  defines 
the  state  transition  matrix,  <£,  using  the  following  equation: 

<P  =  eAd'  (13) 
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Here  dt  is  the  nominal  discrete  time  interval  selected  as  the  smallest  of  all  update  times, 
and  also  as  the  control  time  of  0.125sec.  The  matrix  <t>  will  be  computed  at  the  beginning 
of  the  loop  using  the  most  recent  linearization  of  /.  The  A  matrix  is  reset  at  the  end  of 
every  loop  based  on  the  most  recently  corrected  state  information. 


3.  Q  Matrix  and  System  Noise 

The  Q  matrix  gives  the  filter  information  about  the  expected  levels  of  bias  that  are 
present  when  predicting  the  state.  As  is  evident  in  the  filter  code,  the  Q  matrix  is 
composed,  specifically  of  the  eight  values  of  variance.  Each  value  is  directly  related  to 
its  numerically  corresponding  state  variable.  Each  of  these  values  may  be  adjusted  to 
provided  the  best  final  results.  The  higher  the  value  of  the  variance,  the  less  weight  the 
filter  puts  on  that  particular  measurement.  For  example,  because  the  DGPS  latitude  and 
longitude  are  considered  to  be  “true”  solutions  for  position,  they  are  both  assigned  a 
variance  of  0.  However,  due  to  problems  with  vehicle  magnetic  fields,  it  is  expected  that 
the  compass  measurement  for  heading  will  be  slightly  inaccurate,  therefore  it  is  assigned 
a  variance  of  0.001  rad2. 

Once  a  variance  has  been  assigned  to  each  measurement,  the  Q  matrix  is  then 
created.  The  formation  of  the  Q  matrix  is  shown  below. 


^1  0000000 
0  q2  000000 
00  q3  00000 
000  q4  0000 
0000  q5  000 
00000  q6  00 
000000  q7  0 
0  0  0  0  0  0  0  #8. 
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The  measurement  noise  is  represented  in  the  R  matrix.  The  difference  between 
the  Q  and  the  R  matrix  is  that  Q  tells  the  filter  the  level  of  certainty,  which  it  equates  to 
each  of  the  predicted  state  variables  (9).  R  tells  the  filter  how  much  noise  it  expects  to 
receive  on  each  of  the  measurement  channels  (11).  Both  matrices  use  a  series  of  weights 
to  convey  this  information.  [Ref.  19]  The  system  noise  is  entered  into  the  filter  code  as 
six  separate  measurement  noise  values  Similarly  to  Q,  R  is  formed  assuming 

independence  among  channels. 

nu\  0  0  0  0  0 

0  m/2  0  0  0  0 

0  0  m/3  0  0  0 

R  — 

0  0  0  nu4  0  0 

0  0  0  0  nu5  0 

0  0  0  0  0  m/6. 

By  changing  the  values  of  Q  and  R,  the  filter  may  be  manipulated  to  do  such 
things  as  allow  for  more  or  less  filtering,  which  adds  or  takes  away  from  data  smoothing. 
Additionally,  those  same  values  can  also  speed  up  or  slow  down  the  recursive  process. 

4.  Asynchronous  Data  Processing 

The  output  matrix  should  numerically  represent  conditions  that  are  necessary  to 
ensure  the  filter  program  uses  new  measurement  data  to  determine  the  AUV’s  position. 
It  is  possible  that  the  correct  measurement  information  may  not  be  given  to  the  filter,  due 
to  the  idea  of  asynchronous  data.  This  problem  needs  to  be  specifically  addressed  in 
order  to  keep  the  filter  running  without  errors. 
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a.  Asynchronous  Data  Problem 

The  measurements  coming  from  the  AUV  sensors  are  not  produced  at  the 
same  rate.  This  is  not  to  say  that  the  information  is  produced  out  of  phase,  but  it  is  rather, 
produced  at  different  frequencies.  Specifically,  the  DGPS  data  is  updated  at  a  rate  of 
1Hz,  the  RDI  Doppler  at  2Hz,  and  the  IMU  is  the  fastest  at  100Hz,  but  is  only  sampled 
and  given  to  the  filter  at  8Hz. 

Ideally,  the  navigation  filter  program  should  run  at  a  speed  that  is  faster 
than  the  speed  of  the  fastest  sensor.  This  way  the  filter  can  determine  the  new  position 
before  any  new  measurements  are  taken  from  the  IMU.  However,  because  only  the  IMU 
data  is  updated  every  0.125sec,  a  problem  arises  when  the  filter  tries  to  use  the  old 
measurements  from  other  sensors,  along  with  the  new  IMU  measurement,  when  it 
determines  the  AUV’s  position. 

b.  Asynchronous  Data  Solution 

The  solution  to  this  problem  has  two  parts.  The  first  part  is  filling  in  the 
holes  for  measurements  that  are  not  given  as  often  as  the  most  frequent  one.  The  second 
part  ensures  that  none  of  those  placeholders  are  actually  considered  towards  the 
calculation  of  the  vehicle  position.  Together,  these  two  parts  allow  the  filter  to  adjust 
position  with  every  update  IMU,  or  less  than  every  8Hz. 

To  fill  in  for  data  that  is  not  given  at  8Hz,  there  is  some  preprocessing 
done  in  the  AUV  software.  For  the  ARIES  AUV,  this  is  accomplished  in  the  C  code 
which  parses  incoming  measurement  data  from  each  sensor  before  it  reaches  the  filter,  so 
that  only  the  previously  specified  data,  necessary  for  calculating  vehicle  position,  is  sent 
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to  the  vehicle’s  shared  memory.  This  same  code  ensures  that  measurement  state 
representing  a  sensor  that  did  not  produce  an  updated  measurement,  remains  unchanged 
until  an  update  is  received.  This  way  there  is  a  measurement  present  for  each  part  of  the 
measurement  vector.  The  second  part  of  the  solution  shows  how  the  C  matrix  allows  the 
filter  to  determine  an  updated  position,  when  the  measurements  are  not  always  updated 
themselves. 

If  a  value  is  to  be  kept  in  the  filter  process,  or  considered  useful,  then  the 
C  matrix  coordinate  for  that  data  point  will  be  set  to  1,  and  carried  on  to  the  output.  For 
example: 

y(i  + 1,4)  =  new;  C(4,:)  =  nominal C(4,:); 

Here y(i+l  ,4)  is  the  next  data  point  on  the  4th  measurement  channel. 

If  it  is  not  useful,  the  coordinate  point  is  set  to  0  and  the  information  does 
not  continue  on  any  further.  For  example: 

y(i  + 1,4)  =  old;  C(4,:)  =  0; 

The  following  criteria  are  used  in  the  ARIES  filter  program. 

•  If  the  new  latitude  and  longitude  caused  the  square  root  of  the  error 
covariance,  relative  to  the  previous  data,  to  be  greater  be  greater  than 
1000,  the  new  data  is  not  to  be  used. 

•  If  the  forward  and  lateral  Doppler  speeds  have  an  absolute  value  of  over 
lOm/s,  that  data  is  not  to  be  used. 

•  If  any  new  data  is  equal  to  the  old  data,  the  new  data  is  not  to  be  used. 
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After  each  matrix  is  created  and  manipulated  to  first  predict  and  then 
correct  the  state  estimation,  the  resultant  position  is  presented  to  the  vehicles  tactical 
computer.  From  this  point  the  filter  continues  to  run  over  and  over  until  the  end  of  the 
mission,  and  the  information  it  delivers  to  the  tactical  computer  is  used  to  compose  the 
vehicles  next  plan  of  action,  or  corrective  measure,  to  reach  its  mission  objective. 
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V. 


RESULTS  OF  EXPERIMENTATION 


A.  INTRODUCTION 

Several  independent  periods  of  experimentation  were  required  to  completely 
qualify  the  entire  Navigational  Suite.  Because  the  entire  system  relies  on  the  accuracy  of 
the  DGPS,  the  Suite’s  evaluation  was  broken  down  into  the  qualification  of  its  major 
components,  the  base  DGPS,  and  the  dependant,  recursive  navigational  filter.  This 
qualification  was  accomplished  by  first  evaluating  the  proposed  DGPS  setup,  then  testing 
the  chosen  setup  in  open  water  conditions,  and  finally  configuring  and  testing  the 
Navigational  Filter  in  conjunction  with  the  DGPS  in  those  open  water  conditions.  This 
chapter  will  discuss  the  most  progressive  highlights  of  those  experiments.  It  should  be 
noted  that  due  to  the  experimental  nature  of  this  “first”  testing  sequence,  the  results  seen 
do  not  reflect  the  full  quality  of  the  much-improved  “final”  data,  currently  collected  from 
the  finished  system. 

B.  EVALUATION  OF  DGPS  SETUP 

The  first  set  of  experiments  with  the  ARIES  were  composed  to  merely  ensure  the 
system  components  interacted  as  predicted,  and  were  able  to  produce  acceptable  results. 
These  results  were  carried  out  on  land,  at  a  parking  lot  located  in  front  of  the  Naval 
Postgraduate  School  (NPS)  Center  for  AUV  Research  laboratory.  In  total,  five 
experiments  were  conducted. 

The  Base  Station  was  set  up  near  the  laboratory’s  front  door,  with  the  antenna 
stand  located  outside  in  the  previously  mentioned  parking  lot.  The  vehicle  was  power- 
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up,  so  the  DGPS  components  inside  it  would  be  operating,  and  placed  on  a  flatbed 
loading  cart.  The  cart  was  then  brought  to  the  parking  lot  where  each  experiment  would 
then  be  carried  out.  Finally,  a  GPS  file  was  collected  by  the  AUV’s  CPU.  That 
recording  process  was  initiated  from  the  Base  Station,  running  “Procomm”  on  a  remote 
personal  computer  (PC)  to  login  and  execute  files  inside  the  AUV  computer,  via  radio 
modems.  The  recorded  file  was  later  pulled  from  vehicle’s  CPU  using  a  direct  Ethernet 
cable  (in  the  interest  of  speed,  though  the  file  is  accessible  via  radio  modem),  and 
analyzed  using  several  MATLAB  programs  which  can  be  seen  as  Appendix  B. 

The  data  is  presented  here  in  a  series  of  3  figures  and  a  table  of  DGPS  statistics 
for  each  test.  The  first  figure  is  a  geographical  plot  which  shows  the  GPS  fixes  as  a 
marker,  and  a  track  line  connecting  fix  to  fix.  The  second  figure  is  a  histogram  of  the 
number  of  satellites  detected  by  the  DGPS  while  take  each  fix.  The  final  figure  tracks  the 
DGPS  DOP  during  the  test.  This  figure  shows  comprehensive  DOP  called  Positional 
Dilution  of  Precision  (PDOP),  as  well  as  it  individual  components,  the  Horizontal, 
Vertical,  and  Time  Dilution  of  Precision  (HDOP,  VDOP,  and  TDOP).  Finally,  the  table 
of  statistics  provides  the  total  number  of  recorded  data  points,  the  number  of  those  points 
which  were  accompanied  with  available  differential  corrections,  and  the  number  of  those 
points  which  were  taken  while  the  DGPS  was  in  view  of  5  or  more  satellites. 

It  should  be  noted  that  there  was  some  interference  present  in  the  vehicle’s  ability 
to  receive  signals  from  the  GPS  satellites  due  to  the  overhanging  trees  surrounding  the 
parking  lot,  as  well  as  the  laboratory  itself.  However,  due  to  the  basic  nature  of  these 
experiments,  as  well  as  the  fact  that  the  desired  results  were  produced,  the  experiments 
were  all  held  at  this  location,  and  deemed  successful. 
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1.  Stationary  Test 

The  first  test  conducted  was  a  stationary  test.  The  importance  question  to  be 
answered  during  this  test  is  not  “Will  the  vehicle’s  position  drift,  or  change  gradually”, 
but  instead,  “How  much  will  it  drift?”  Because  the  system  uses  the  GPS  signal  offered  to 
the  public,  as  discussed  previously,  some  drift  will  occur.  However,  the  vehicle  will  be 
required  in  some  missions,  to  find  and  maintain  a  specific  point  on  the  globe  with  only 
DGPS  information.  Therefore  the  AUV  Navigational  Suite  needs  to  be  able  to  identify  a 
specific  set  of  DGPS  coordinates,  and  maintain  them  with  little  variation,  regardless  of 
the  quality  or  SS  it  receives. 
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Figure  5.1  shows  the  results  of  the  Stationary  Test.  These  results  were  obtained 
by  leaving  the  vehicle  static  for  approximately  440sec,  or  7.33min.  During  that  time,  the 
vehicle  drift  is  significant,  but  no  more  than  expected.  The  center  point  of  the  data,  in 
this  case  is  not  the  same  as  the  origin  of  the  plot’s  axes. 

The  program,  which  analyzed  the  data,  is  instructed  to  label  the  axis  origins  at  the 
position  of  the  data  points  located  farthest  east,  and  farthest  south.  However,  due  to  some 
environmental  interference,  those  data  points  may  not  be  the  most  accurate  measurements 
of  vehicle  position  possible.  Thus  the  center  point  of  the  data  is  determined,  and  the  drift 
results  were  compared  to  that  point. 

The  E-W  drift  of  the  vehicle  data  is  over  a  range  of  4  meters,  but  with  a  variation 
of  2m  from  the  observed  center  point  of  the  data.  However  the  N-S  drift  is  within  a  range 
of  26.5m,  with  a  max  variation  of  19.2m  from  the  center  point.  This  is  mainly  due  to 
only  a  few  errand  data  points,  which  is  caused  by  a  drop  in  the  satellite  signals.  When  the 
number  of  satellites  the  vehicle  can  see  drops  below  5,  the  vehicle  is  forced  to  use  exactly 
the  information  it  gets,  and  is  not  able  to  choose  the  best  data  from  the  satellites  with  the 
best  DOP.  Figures  5.2  and  5.3  show  the  data’s  histogram  for  the  number  of  satellites,  as 
well  as  a  plot  of  the  DOP  with  respect  to  time. 
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The  histogram  shows  the  only  1 1  data  points  had  less  than  5  satellites  in  view  when  the 
position  was  recorded.  The  DOP  plots  show  that  excluding  those  1 1  data  points,  the 
dilution  of  precision  remains  under  five  for  the  entire  data  set.  This  means  that  on  a  scale 
of  0  to  100  for  probability  of  error  content,  the  data  set  remains  around  5. 

Though  the  geographic  plot  does  indeed  show  a  few  errand  points,  the  large 
majority  of  the  points  are  within  the  desired  speculations.  In  fact.  Table  5.1  below  shows 
the  most  important  data  obtained  from  this  experiment. 


— 

HjggM 

The  important  information  to  note  is  that  out  of  the  440  fixes  recorded,  85  percent 
of  them  were  within  the  requirements.  Figure  5.4  shows  that  if  the  remaining  14.1%  of 
the  data  points  were  removed,  which  can  be  done  by  raising  the  G-12’s  threshold  for 
accepting  the  number  of  satellites,  the  stationary  plot  returns  results  within  2m  precision. 
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2.  Motion  Tests 


The  next  test  carried  out  in  the  parking  lot  was  a  simple  motion  test.  The  purpose 
of  this  test  was  to  find  out  if  the  DGPS  could  keep  a  signal  between  the  Base  Station  and 
the  AUV  while  it  was  in  motion.  Two  supplementary  things  examined  during  this  test 
were  the  vehicle’s  ability  to  track  satellites  while  in  motion,  as  well  as  whether  or  not  is 
was  capable  to  distinguish  a  specific  shape  created  by  the  vehicle  path. 

The  path  taken  by  the  vehicle  was  planned  to  be  a  circular  pattern.  Table  5.2 
below  shows  the  statistics  of  the  data  set.  This  time,  those  points  without  differential 
corrections,  or  less  than  five  satellites  have  been  recorded  initially,  but  where  filtered  out 
of  the  geographic  positioning. 


|  TABLE  5.2:  Motion  Test  Statistics  | 

Number  of  Fixes 

Number  with  Differential 
Number  with  >4  Satellites 
Percentage  of  Useful  Data 

348 

262 

.  262 

During  this  test,  the  percentage  of  useful  data  declined  slightly,  however,  the 
number  of  data  points  taken  did  as  well.  Therefore  is  could  be  said  that  the  number  of 
bad  data  points  per  run  could  be  somewhat  constant,  due  to  the  vehicles  path  through  a 
specific  part  of  the  parking  lot.  Figure  5.5  shows  the  geographic  plot  of  the  track,  while 
figures  5.6  and  5.7  show  the  Satellite  Histogram,  and  the  DOP  plot. 
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The  geographic  plot  shows  that  the  vehicle  clearly  made  an  incomplete  circular 
pattern.  Also,  during  this  test  the  vehicle  maintained  a  DOP  below  6,  and  was  able  to  use 
75%  of  its  recorded  data  points.  Therefore  the  DGPS  shows  it  can  operate  under 
conditions  of  motion,  and  the  test  was  considered  to  be  successful  in  meeting  its 
objectives. 

3.  Shape  Test 

The  final  planned  vehicle  test  was  intended  to  see  if  the  path  recorded  by  the  CPU 
could  be  used  to  accurately  measure  a  physical  distance.  The  previous  test  had  proven 
that  the  physical  shape  of  the  path  could  indeed  be  recognized,  but  how  much  did  the 
dimensions  of  that  shape  suffer  due  to  inaccuracies  in  the  GPS  coordinates. 

While  carrying  out  the  test,  a  square  path  of  the  dimensions  36x24ft  was  marked 
off  in  the  parking  lot.  The  vehicle  was  then  towed  around  this  track.  Again  it  should  be 
noted  that  due  to  the  size  of  the  track,  and  the  size  of  the  parking  lot,  the  results  of  the 
southern  most  path  of  the  track  are  somewhat  plagued  by  the  vehicle’s  immediate 
proximity  to  the  laboratory.  However,  the  statistics,  as  seen  in  Table  5.3,  show  that  the 
vehicle  was  still  able  to  use  76%  of  its  recorded  data. 


|  TABLE  5.3:  Shape  Test  Statistics  | 

;  Number  of  Fixes 

Number  with  Differential 

Number  with  >4  Satellites 

mmm 

Percentage  of  Useful  Data 

60 


Geographic  Plot  of  GPS  Track  (Rectangular) 


Figure  5.8:  Shape  Test  Geographic  Plot, 


Frequency 


Satelite  Frequency 


Number  of  Satelites 


Figure  5.9:  Histogram  of  Shape  Test  Satellites. 
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The  AUV’s  geographic,  histogram,  and  DOP  plots  (Figures  5.8,  5.9,  and  5.10) 
show  that  the  vehicle  was  able  to  measure  the  path  length  closely.  It  can  also  be  easily 
seen  that  the  tracks  compose  the  four  sides  of  the  box.  An  analysis  using  linear  regression 
shows  the  data  exhibits  a  standard  deviation  better  than  30cm.  The  histogram  shows  at 
least  5  satellites  while  positioning,  and  the  DOP  shows  no  greater  than  5.25. 

The  results  from  these  tests  very  considered  satisfactory  enough  to  allow  the 
DGPS  setup  to  be  accepted.  The  system  was  next  moved  to  the  Monterey  Bay  for  its 
official  qualification  in  the  open-water. 

C.  OPEN-WATER  QUALIFICATION  OF  THE  DGPS 

The  objective  of  the  DGPS  open  water  tests  was  to  determine  if  the  existing  setup 
could  operate  effectively  in  the  ocean.  “Effective”  operation  means  that  the  AUV  will  be 
able  to  maintain  contact  with  the  satellites  while  on  the  water’s  surface,  and  effectively 
track  its  position.  If  this  objective  could  not  be  met,  then  the  setup  would  have  to  be 
altered. 

A  specific  concern  to  be  observed  in  the  results,  was  AUV’s  ability  to  receive 
signals  from  all  associated  sources,  while  being  located  so  close  to  the  water.  Due  to  the 
fact  that  both  the  command/control/differential  and  GPS  antennae  are  both  approximately 
4  to  12in  out  of  the  water,  while  the  vehicle  is  located  on  the  surface  during  an  average 
Sea  State  of  2,  either  may  not  be  able  to  distinguish  a  strong  enough  signal. 

The  tests  were  conducted  in  the  Monterey  Bay,  at  the  Public  wharf.  The  AUV 
was  loaded  at  the  lab  and  brought  to  the  wharf  in  a  trailer.  A  public  crane  at  the  wharf 
was  used  to  lower  the  AUV  into  the  water,  where  it  would  then  be  towed  out  into  the 
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open  water  by  a  two  man  team  or  researchers  in  a  Zodiac  inflatable  boat.  Once 
positioned  in  the  open  water,  and  powered  by  triggering  it’s  magnetic  switch,  the  vehicle 
CPU  was  then  commanded  to  begin  recording  data  from  the  Base  Station  CPU. 

The  Base  Station  was  located  half  way  along,  and  on  top  of  the  peer, 
approximately  15-20ft  above  sea  level.  From  this  vantage  point,  the  Base  Station  could 
remain  in  contact  with  the  AUV  for  transmitting  command  and  control  information,  as 
well  as  differential  corrections. 

The  differential  corrections  and  GPS  information  from  satellites  were  received 
and  recorded  by  the  AUV,  and  downloaded  the  end  of  the  mission.  The  vehicle  was 
towed,  on  the  surface  of  the  water,  during  the  entire  test. 

1.  First  Test  Results 

During  the  first  test  in  open  water  it  was  discovered  that  the  AUV  DPGS  tracked 
the  vehicle’s  path  very  well  for  the  first  radial  mile.  Figure  5.11  shows  the  exact 
geographic  path  the  AUV  took  during  the  test. 
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This  plot  shows  that  the  AUV  was  towed  in  a  triangular  pattern,  during  which  its 
Navigational  System  was  able  to  accurately  position  the  vehicle  during  the  majority  of 
the  run.  However,  at  the  eastern  most  comer  of  the  plot,  the  vehicle  was  not  able  to 
receive  many  DGPS  fixes  along  its  track. 

The  reason  for  this  lack  of  DGPS  positioning,  is  found  with  a  closer  analysis  of 
the  system  which  provides  differential  corrections.  After  checking  the  results  for  any 
specific  problems,  it  became  apparent  that  at  the  eastern  most  comer,  the  vehicles  farthest 
point  from  the  Base  Station,  the  recorded  data  had  not  received  any  differential 
corrections.  .  This  indicates  that  there  is  a  range  limitation  of  the  differential  correction 
radio  link. 

However,  it  was  also  noted  that  there  was  a  loss  of  command  and  control  at  that 

4 

time  as  well.  This  was  well  within  the  previously  tested  radio  range  of  the  vehicle.  Since 
both  the  command  and  control,  and  the  differential  corrections  systems  share  the  two- 
way  antenna  through  a  75ohm  combination/splitter  called  a  “Mulitplexer”,  one  of  these 
components  must  be  the  culprit.  Table  5.4  below  shows  exactly  how  that  data  effected  the 
vehicle’s  percentage  of  useful  data. 


|  TABLE  5.4:  Open  Water  Test  Statistics  | 

Number  of  Fixes 

Number  with  Differential 

Number  with  >4  Satellites 
Percentage  of  Useful  Data 

3496 

2508 

62.8 

Because  the  data  that  does  not  show  the  use  of  differential  corrections  is  not 
considered  useful  and/or  plotted  at  this  stage  in  the  testing,  The  628  fixes  taken  during  the 
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signal  outage  were  lost  when  the  filter  discarded  all  GPS  fixes  that  did  not  have 
differential  corrections.  Therefore,  they  were  unable  to  be  plotted,  but  were  still 
represented  in  Table  5.4.  It  should  be  noted  that  the  actual  Navigation  Filter  used  in  the 
AUV  accepts  raw  GPS  and  DGPS  data,  but  weights  them  for  confidence  using 
predetermined  error  bounds,  which  consider  if  differential  corrections  are  present  or  not. 
However,  for  testing  purposes,  where  the  objective  is  the  highest  standard  possible, 
regular  GPS  fixes  are  less  than  ideal  and  therefore  not  considered. 

Acknowledging  the  fact  that  the  Sea  State  on  this  day  was  only  a  1,  the  small 
aerial  was  able  to  receive  position  information  from  the  satellites  for  the  majority  of  the 
rim.  However,  Figures  5.12  and  5.13  show  results  that  both  support  and  conflict  the  idea 
that  the  aerial  was  working  as  well  as  it  could  have  been. 
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The  satellite  histogram  shows  that  the  AUV  was  able  to  pick  up  a  mean  of  6 
satellites,  with  a  maximum  of  9  satellites,  during  the  run.  However,  as  was  seen  in  Table 
4.4,  only  62%  of  the  data  had  five  or  more  satellites.  In  the  open  ocean,  5  or  more 
satellites  should  be  visible  at  all  times.  This  is  the  first  indication  of  a  potential  problem 
occurring  in  relation  to  the  aerial. 

The  DOP  figure  supports  the  latter  fact  by  showing  that  even  though  DOP  values 
average  about  3.3,  there  is  a  greater  frequency  of  the  occurrence  of  DOP  peeks,  which 
range  as  high  as  11.8.  This  data  works  in  conjunction  with  the  fact  that  aerial  was 
repeatedly  unable  to  receive  a  signal  from  more  than  5  satellites.  The  GPS  receiver  was 
therefore  forced  to  except  those  signals  as  the  best  possible,  for  triangulation.  Once 
again,  because  better  results  we  expected  from  the  GPS  aerial,  several  changes  were 
made  before  the  next  test. 

2.  System  Troubleshooting 

A  closer  examination  of  the  two-way  antenna  found  it  to  be  faulty.  The 
connection  at  the  internal  base  of  the  antenna  was  loose,  which  caused  a  severe  decrease 
in  its  receiving  range.  It  was  replaced  before  the  next  experiment  by  a  similar  antenna 
manufactured  by  the  same  company.  The  only  difference  between  the  new  and  old 
antennae  was  that  the  design  had  been  modified  so  that  the  new  antenna  was  only  12 
inches  long,  vice  24.  The  GPS  aerial  was  also  examined  for  potential  problem  sources,  as 
well  as  possible  better  methods  for  its  implementation.  This  examination  identified  two 
problems. 
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First,  due  to  the  series  of  several  connections  and  cables  between  the  aerial  and 
GPS  receiver,  including  a  hull  connection,  the  aerial’s  sensitivity  was  being  reduced  due 
to  impedance  mismatching.  To  rectify  this  situation,  the  hull  connection,  as  well  as  the 
internal  connecting  cables,  were  removed.  A  single  cable  connecting  the  aerial  directly 
to  the  GPS  receiver  was  then  added,  and  passed  through  a  new  hull  penetration.  The 
difference  between  a  hull  connection  and  a  hull  penetration  is  that  there  is  no  break  in  the 
line  through  a  sealed  penetration.  The  second  change  made  to  the  aerial  was  the  addition 
of  a  ground  plate. 

Considering  that  the  previously  mention  SS  problem  may  have  indeed  been 
caused  by  the  aerial  proximity  to  the  water,  a  ground  plate  would  provide  optimum  signal 
reception  for  the  aerial  by  reducing  any  signal  reflection  from  the  water’s  surface. 

3.  Second  Test  Results 

After  the  modifications  were  made  to  the  ARIES ’s  DGPS,  the  tracking  results 
observed,  during  all  subsequent  tests,  showed  a  very  high  level  of  improvement.  The 
results  of  one  such  test  show  this  improvement,  boasting  100%  production  of  useful 
DGPS  fixes  with  5  or  more  satellites.  Table  5.5  below  shows  the  data’s  statistics. 


1  TABLE  5.5:  Open  Water  Test2  Statistics  | 

Number  of  Fixes 

Number  with  Differential  . 

Number  with  4<  Satellites 
Percentage  of  Useful  Data^. 

§2122291 

wfiimwi 
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This  particular  test  was  no  different  than  the  previous  open  water  test,  with  regards  to 
environmental  conditions  or  procedure.  However,  it  is  noticeable  that  during  this  run,  the 
system  was  able  to  maintain  contact  with  the  Base  Station,  as  well  as  receive  good  GPS 
satellite  data  from  a  large  number  of  satellites  then  entire  time.  Figures  5.14,  5.15,  and 
5.16  provide  more  detailed  information  about  the  test. 

The  geographic  plot  shows  that  during  the  test,  the  AUV  was  simply  towed  out  in 
one  direction  (NE),  and  then  towed  back.  As  mentioned  earlier,  the  Sea  State  was  no 
different  than  the  previous  open  water  test.  However,  the  figure  shows  there  was  no 
point  along  the  track  (blue  line)  which  was  not  covered  by  GPS  fixes  (red  points). 

The  satellite  histogram  shows  the  GPS  aerial  was  able  to  receive  a  strong  signal 
from  a  mean  of  8  satellites  throughout  the  length  of  the  track.  Due  to  this  fact,  the  DGPS 
was  also  able  to  maintain  good  DOP  during  the  test,  which  is  shown  in  Figure  5.16.  The 
average  DOP  value  is  1.8,  which  is  a  43%  decrease  over  the  first  open  water  test,  and 
never  rises  over  6.9,  a  42%  decrease. 
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The  results  of  this  test  clearly  qualify  the  DGPS  for  operation  in  the  open  water 
environment.  The  one  aspect  of  testing  which  was  not  observable  from  this  test, 
however,  was  the  extended  range  of  the  DGPS  from  the  Base  Station,  with  the 
modifications  to  the  antenna.  This  range  however  was  lengthened  to  the  original 
expected  value,  over  1.5  miles.  This  will  be  noticeable  in  the  geographic  plots  of  the 
filter  tests,  which  were  carried  out  after  the  open  water  test.  These  tests  allow  the  DGPS 
to  work  in  conjunction  with  the  other  components  of  the  Navigational  Suite,  towards  the 
comprehensive  control  of  the  vehicle. 

D.  EVALUATION  OF  THE  DGPS  BASED  NAVIGATIONAL  FILTER 

The  Navigational  Filter  tests  intended  to  evaluate  the  degree  of  accuracy  with 
which  the  filter  program  could  estimate  the  geographical  position  and  orientation  of  the 
ARIES  AUV.  This  objective  was  met  by  attempting  to  prove  two  specific  points.  First, 
the  Navigational  Filter  provides  a  better  estimate  of  the  vehicle’s  position  than  a  system, 
which  used  only  DGPS  or  dead-reckoning  alone,  would  provide.  Secondly,  the 
properties  of  the  filter  can  be  adjusted  to  reduce  geographic  error  on  long  underwater 
runs,  to  nearly  the  precision  attained  with  surface  navigation.  Once  again,  all  tests  were 
carried  out  in  the  Monterey  Bay.  However,  there  were  several  differences  between  the 
filter  and  GPS  tests. 

All  of  the  described  components  in  the  Navigational  Suite  were  operational 
during  these  tests.  Each  sensor  provided  synchronized  data  to  a  time  stamped  file  during 
the  AUV’s  run.  This  data  was  then  pulled  from  the  AUV  and  analyzed  using  MATLAB 
code  that  simulates  the  ARIES  Navigational  Filter.  [Ref.  20]  The  modified  version  of 
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this  code,  which  is  used  during  these  tests,  can  be  seen  as  Appendix  C.  Unlike  the 
previous  DGPS  tests,  it  is  necessary  to  run  all  of  the  vehicle’s  navigational  sensors  for  the 
filter  to  estimate  position. 

In  addition,  during  several  tests  in  which  dives  were  conducted,  the  vehicle’s 
thrusters  and  control  planes  where  operational.  The  vehicle  was  commanded  to  perform 
self-propelled  dives  for  specified  time  intervals  to  observe  the  filter’s  behavior  with  no 
DGPS  data.  Such  tests  also  proved  useful  for  observing  the  resultant  time  it  took  the 
filter  to  estimate  the  vehicle’s  position  after  DGPS  was  reacquired,  as  well  as  providing 
visual  foundation  for  the  specific  kinds  of  filter  property  adjustments  that  would  be 
beneficial  for  AUV  positioning. 

1.  Navigation  Filter 

The  first  objective  was  to  prove  that  the  filter  could  provide  better  position 
estimates  than  a  system,  which  uses  only  DGPS  or  dead  reckoning.  Therefore  two  tests, 
one  entirely  on  the  surface,  and  the  other  which  contained  a  series  of  AUV  dives,  were 
performed  to  provide  a  comparison  of  the  separate  navigation  methods. 

The  first  test  simply  towed  the  vehicle  from  “Fisherman’s  Wharf’  in  Monterey, 
out  into  the  bay  and  then  back  again.  During  this  run,  the  time  stamped  file  was  filled 
with  navigational  data  by  the  sensors,  and  afterwards  the  results  were  observed  for 
accuracy.  The  figure  below  shows  a  plot  of  the  vehicle’s  track. 
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The  blue  points  seen  in  Figure  5.17,  are  the  DGPS  fixes  that  were  taken  by  the 
AUV  along  the  track.  The  green  points  are  the  Filter’s  estimate  of  the  track.  Because  the 
DGPS  provides  a  geographic  fix  for  the  AUV  every  second,  with  sub-meter  accuracy  as 
discussed  earlier,  the  filter  weights  the  DGPS  solution  very  heavily.  Therefore,  the  filter 
is  able  to  track  the  AUV’s  position  exactly  along  with  the  DGPS  fixes.  At  the  same  time, 
the  filter  is  able  to  learn  a  little  about  the  biases  present  in  its  own  sensors. 

As  the  sensors  send  data  to  the  filter,  it  keeps  track  of  exactly  where  the  compass, 
Acoustic  Doppler,  and  IMU  say  it  should  be  positioned,  using  a  dead  reckoning  solution. 
This  occurs  even  if  the  dead  reckoning  solution  does  not  agree  with  the  DGPS  solution. 
When  these  solutions  do  not  agree,  because  the  DGPS  has  been  weighted  more  heavily,  it 
is  accepted  as  the  “true”  solution.  Therefore,  the  filter  is  now  able  to  measure  the  error 
between  the  two  solutions,  and  correctly  attribute  it  to  the  two  biases  it  has  estimated. 
Figure  5.18  shows  the  heading  the  filter  estimated  as  it  compared  to  the  compass  heading, 
and  Figures  5.19  and  5.20  show  the  resultant  measure  of  the  yaw  rate,  and  compass  bias 
estimated  by  the  filter  during  the  test. 
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Figure  5.18:  Filter  Test  #1  AUV  Heading. 
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The  heading  plot  not  only  compares  the  estimated  heading  with  the  compass 
heading,  but  it  also  demonstrates  the  “wrap”  count  method,  which  was  previously 
discussed,  and  is  not  handled  by  the  filter  code  data  preprocessing.  Even  though  the 
figure  appears  to  show  an  almost  exact  match  between  the  two  heading  plots,  the 
compass  bias  plot  shows  that  even  though  the  trends  are  exactly  the  same,  the  compass 
and  the  filter  values  differ  as  much  as  20.4°.  This  also  shows  that  the  filter  is  tracking  the 
vehicle,  and  still  has  room  for  adjustments. 

However,  the  yaw  rate  figure  shows  the  IMU  to  be  doing  very  well  in  its 
measurements.  The  yaw  rate  bias  has  a  mean  value  of  4.3*  10'3  over  the  entire  run.  This 
can  be  considered  negligible  in  light  of  the  compass  bias.  Ideally,  with  low  bias 
estimates  for  the  compass  and  yaw  rate,  the  error  covariance  of  the  state  variables  will  be 
decreasing  throughout  the  entire  run.  This  decrease  is  simply  due  to  the  fact  that  the  filter 
is  learning  what  bias  needs  to  be  corrected  in  its  next  estimate.  In  Figure  5.21  you  can 
see  that  the  general  trend  in  the  first  two  elements  of  the  error  covariance,  X  and  Y,  is  a 
decline  in  value.  This  plot  specifically  shows  the  error  on  X  and  Y  in  terms  of  the 
deviation  ax  and  ay  in  m  . 
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First  and  Second  Covariance  Elements 


Figure  5.21:  Filter  Test  #1  Covariance, 


Decreasing  error  covariance  shows  that  the  filter  is  converging  to  a  least  squares  solution. 
Likewise,  the  path  plot  shows  that  it  also  accurately  tracks  the  vehicle.  Because  there  is 
measurable  bias  between  the  filter’s  dead  reckoned  and  DGPS  solutions,  the  filter’s 
output,  the  DGPS  solution,  can  be  considered  more  accurate  than  the  output  of  a 
navigation  system  which  uses  only  dead  reckoning.  However,  the  filter  output  is  only 
better  than  dead  reckoning  when  it  receives  the  DGPS  solution. 

Since  GPS  satellite  data  does  not  travel  underwater,  the  filter  must  rely  on  a  dead 
reckoning  solution  when  the  GPS  signal  is  no  longer  detectable.  At  this  point  the  filter  is 
as  good  as  a  dead  reckoning  system.  But,  it  is  now  better  than  a  system  that  uses  only 
DGPS  for  navigation,  because  such  a  system  cannot  go  underwater  and  simultaneously 
maintain  an  accurate  position  estimate.  The  second  test,  a  dive  test,  shows  an  example  of 
when  the  filter  has  to  use  the  dead  reckoning  solution. 

Before  the  dive,  the  filter  learns  bias  information  about  the  compass  and  gyro 
measurements.  It  uses  these  learned  biases  to  help  estimate  the  vehicle’s  position  while  it 
is  underwater.  When  the  AUV  surfaces  and  begins  taking  DGPS  fixes,  the  filter  quickly 
corrects  to  the  “true”  solution  from  the  dead  reckoning  solution  it  had  been  following. 
Figure  5.22  shows  a  segment  of  the  dive  run  which  exhibits  this  situation. 
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Figure  5.22:  Filter  Test  #2  DGPS  and  Filter  Track. 
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The  vehicle  has  just  surfaced  from  a  dive,  and  finds  it  is  6m  off  from  its  estimated  dead 
reckoning  position.  It  immediately  corrects  to  the  DGPS  position  just  before  the 
beginning  of  the  second  dive.  The  6m  error  in  position  is  due  to  the  fact  that  just  before 
the  AUV  went  under,  it  learned  the  bias  for  a  heading  90°  off  from  its  intended  heading. 
After  it  went  underwater,  the  AUV  did  a  corrected  itself  to  drive  on  its  intended  path, 
which  was  a  heading  it  had  learned  very  little  bias  information  about. 

Surfacing  after  the  second  dive  however,  the  vehicle  finds  it  is  much  closer  to 
where  it  thought  it  was.  Without  looking  at  the  path  plot,  this  fact  is  very  obvious  when 
viewing  the  covariance  curves  for  the  entire  test,  Figure  5.23.  It  shows  areas  of  low 
covariance  when  the  AUV  is  on  the  surface  collecting  DGPS  data.  However,  as  soon  as 
DGPS  is  lost,  the  error  begins  steadily  rising.  The  viewable  difference  is  that  the  peak  for 
the  second  dive  is  much  smaller  than  the  first,  655m2  smaller. 

On  that  same  figure  is  a  record  of  the  vehicles  altitude.  By  comparing  the 
vehicles  return  to  the  surface  with  the  drop  in  covariance,  it  is  possible  to  see  that  the 
filter  can  correct  its  position  as  soon  as  it  begins  receiving  DGPS  signals.  During  this 
run,  the  time  from  surface  to  fix  was  5.2  seconds. 
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Figure  5.23:  Filter  Test  #2  Covariance  and  Depth. 


By  analyzing  the  data  from  both  filter  tests,  it  may  be  easily  stated  that  the  ARIES 
Navigational  Filter  out  performs  any  potential  navigation  system  that  operated  using  only 
DGPS  or  dead  reckoning.  Therefore  the  filter  met  the  first  objective  set  to  qualify  it  in 
the  ARIES  AUV.  Meeting  the  second  objective  will  determine  how  accurate  the  filter’s 
solution  can  be  without  the  use  of  a  DGPS  signal. 

2.  Filter  Adjustments 

This  final  section  of  results  will  examine  a  study  on  the  effects  of  adjusting  the 
properties  within  the  filter  to  enhance  its  performance.  The  set  objective  for  these 
adjustments  is  to  increase  the  vehicle’s  accuracy  of  position  estimation  at  the  end  of 
powered  dive  runs.  This  objective  can  theoretically  be  met  by  adjusting  the  speed  and 
precision  the  filter  uses  to  predict  and  correct  the  vehicle’s  state.  The  filter’s  speed  and 
precision  can  be  changed  by  changing  the  values  of  the  gains  which  govern  the  Q  and  R 
matrices  previously  discussed  in  chapter  4. 

The  data  used  during  this  study  is  composed  of  two  separate  dive  tests  conducted 
by  the  ARIES.  One  test  involves  the  ARIES  making  five  powered  dives  along  on 
continuous  path,  which  it  was  towed  around  between  dives.  The  other  data  set  tracks  the 
AUV  as  it  makes  a  dive  under  vehicle  propulsion,  which  was  immediately  followed  by  a 
powered  surface  run,  and  then  another  powered  dive.  These  two  data  sets  were  selected 
for  two  specific  reasons. 

First,  the  data  used  to  adjust  the  filter  properties  must  contain  dive  exercises.  It  is 
necessary  to  examine  a  dive  to  ensure  the  filter  attains  the  most  accurate  position 
estimation  possible  when  the  vehicle  surfaces.  Additionally,  because  one  data  set 
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involves  the  vehicle  being  towed,  and  the  second  tracks  the  vehicle  entirely  under  its  own 
propulsion,  together  the  data  sets  are  different  enough  to  provide  an  objective  test  of  the 
filter  results.  The  results  however,  were  specifically  examined  according  to  the  changing 
gain  values  and  not  the  data  sets. 

The  first  step  was  to  narrow  down  the  numerical  region  of  gains  to  be  tested.  The 
initial  values  for  the  gains  were  as  follows: 
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In  these  equations,  Gq  is  1  and  Gr  is  1000,  originally.  To  narrow  down  the 
possible  options  for  gain  adjustments,  G  was  changed  by  powers  of  ten,  for  the  vehicle 
run  which  was  entirely  under  power,  to  decided  which  power  of  ten  yielded  the  best 
results.  The  geographic  plot  for  the  data  set,  using  the  original  gains  may  be  viewed  as 
Figure  5.24. 
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Figure  5.24:  Gain  Test  Data  Set  #2  DGPS  and  Original  Filter  Track. 
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Table  5.6  below  shows  the  results  changing  the  gain  had  on  the  positional 
accuracy  of  the  plot.  This  result  occurs,  due  to  the  fact  that  the  higher  the  values  of  the  R 
matrix,  the  more  system  noise  the  filter  considers  present  in  the  measurement  model. 
Therefore,  with  more  noise  present,  the  filter  does  not  “filter”  as  much,  because  it  has  a 
reduced  sensitivity  to  small  changes  in  the  data. 


1  TABLE  5.6:  Initial  R  Gain  Test  Results  1 

msESMWumm 

1  Position  Error  (ml 

1  1000  |  1 

2.00 

fjgjfl 

11 

2.50  | 

The  “Position  Error”  is  the  linear  distance  between  the  filter’s  estimated  position 
at  the  point  of  the  vehicle’s  resurface  from  the  second  dive,  and  the  first  DGPS  fix.  This 
data  shows  that  the  ideal  range  for  the  R  matrix  gain  is  between  10  and  100  to  continue 
pursuing  continuous  sub-meter  accuracy.  The  next  test,  similar  to  the  first,  measures  the 
results  that  occur  do  to  changes  in  the  value  of  the  Q  matrix  gain. 

This  analysis  was  conducted  using  the  data  from  the  5  dive  test,  with  towing  in 
between  due  to  the  extensive  vehicle  path.  A  more  extensive  path  makes  greater  use  of 
the  RDI  compass,  and  since  the  Q  matrix  governs  the  speed  of  the  filter,  it  is  directly 
related  to  the  bias  measurement.  A  higher  gain  value  should  run  the  filter  faster,  and 
therefore  identify  a  higher  bias  value,  perhaps  to  the  point  of  over  correction.  The 
original  path  plot  of  this  dive  test  can  be  seen  as  Figure  5.25. 
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Figure  5.25:  Gain  Test  Data  Set  #1  DGPS  and  Original  Filter  Track. 
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The  results  of  the  analysis,  seen  in  Table  5.7  below,  show  yet  another  narrowing 
of  the  possible  filter  options.  The  position  error,  which  is  a  sum  of  the  position  errors  for 
all  5  dives,  and  the  max  compass  bias,  which  is  the  absolute  range  of  compass  bias,  show 
that  the  best  combined  results  occur  when  the  Q  gain  is  between  1  and  0.1.  When  the 
gain  is  between  this  region  it  has  the  lowest  error,  and  the  compass  bias  is  still  measured 
precisely,  but  found  to  be  relatively  low. 


i  TABLE  5.7:  Initial  O  Gain  Test  Results  1 
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Therefore  the  next  analysis  combines  the  gains  values  of  100,  50  and  10  for  use 
with  the  R  matrix,  and  1,  0.5,  and  0.1  for  use  with  the  Q  matrix.  Each  combination  was 
compared  for  positional  accuracy  and  the  amount  of  measured  compass  bias.  Those 
results  yielded  obvious  trends,  which  indicate  the  R  gain  of  50  performs  the  best  with  all 
three  Q  gain  values.  However,  the  results  from  within  the  different  Q  gains  differed  little. 
The  only  noticeable  difference  followed  the  general  trend  that  a  lower  Q  gain  will 
identify  a  lower  range  of  compass  bias.  Table  5.8  shows  this  trend. 
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1  TABLE  5.8:  Final  Gain  Test  Results  I 

|  Q  earn  |  Position  Error  tm)  Data  #1  &#2  1  “ax  ^  Bias  | 

p . 

50 

0.1  1  0.67  |  19.33  1  3.61  ! 

As  a  result,  the  final  gains  chosen  for  the  filter,  was  the  R  gain  value  of  50,  and 
the  Q  gain  value  of  1 .  The  plotted  results  of  this  choice  for  data  set  #1  can  be  seen  Figure 
5.26.  The  reason  the  Q  gain  with  the  lower  compass  bias  was  not  selected  as  default,  was 
because  position  error  holds  a  higher  priority  towards  the  success  of  ARIES  AUV  and  its 
missions. 

These  tests  show  without  a  doubt,  that  by  adjusting  the  properties  of  the  filter,  its 
performance  can  be  enhanced.  It  is  also  visible  from  the  results,  that  on  longer 
underwater  runs,  such  as  it  is  with  data  set  #1,  the  position  error  can  be  corrected  to  sub¬ 
meter  values.  These  corrections  will  allow  the  vehicle  to  progressively  expand  its 
navigational  ability.  Ideally,  shorter  runs  and  runs  with  more  complicated  underwater 
maneuvers  will  also  be  able  to  create  surface  worthy  positional  accuracy  on  largely 
submerged  paths. 
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Figure  5.26:  Gain  Test  Data  Set  #1  DGPS  and  Tuned  Filter  Track. 
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VI.  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  CONCLUSIONS 

As  AUVs  continue  to  become  a  better  option  for  ocean  data  gathering  operations, 
research  will  continue  to  strive  for  their  improved  performance,  and  those  who  use  them 
will  strive  to  decrease  their  cost.  There  will  always  be  a  reason  to  enhance  the  methods 
used  for  underwater  positioning  in  almost  any  underwater  field  of  study.  The  continued 
development  and  evaluation  of  “small,  low  power,  low  cost”  underwater  Navigational 
Suite  will  always  be  of  interest.  Therefore  the  experimental  configuration  and  evaluation 
of  one  such  system  for  the  ARIES  AUV  has  been  the  focus  of  this  work. 

Through  a  progressive  series  of  testing,  this  work  has  shown  that  the  ARIES 
DGPS  based  Navigation  Suite  is  capable  of  providing  a  solution  for  the  vehicle’s  position 
with  sub-meter  precision.  This  precision  has  been  obtained  for  relatively  short  dives 
(200-300m  underwater  runs)  using  a  properly  tuned  EKF.  Additionally,  it  has  been  found 
that  only  a  2-5sec  delay  occurs  for  the  reacquisition  of  GPS  satellite  signals  after 
surfacing.  This  minimal  amount  of  time  allowed  the  Suite  to  quickly  “learn”  and  adjust 
from  sensor  bias  errors. 

B.  RECOMMENDATIONS 

Based  on  the  results  of  this  thesis  and  the  research  required  to  complete  it,  an 
initial  recommendation  should  be  made  to  continue  the  study  of  DGPS/IMU/Doppler 
navigation.  With  further  time  and  experimentation,  the  Suite  can  be  relied  on  more 
heavily  for  more  complex  future,  underwater  operations.  Based  on  its  ability  to 
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effectively  position  the  vehicle,  operations,  which  will  allow  the  vehicle  to  locate  and 
examine  underwater  features  and  targets  such  as  in  mine  countermeasure  operations  or 
ocean  floor  studies,  could  now  be  accomplished  with  a  higher  proficiency.  Attention 
should  also  continue  to  be  given  to  the  technology  used  within  the  Suite.  As  positioning 
becomes  a  greater  interest  in  the  underwater  arena,  as  well  as  the  automotive  and  forestry 
industry,  better  technology  will  become  available  for  less,  as  the  “cost/accuracy  curve” 
decreases.  [Ref.  3]  This  trend  should  be  fully  utilized,  to  aide  in  research  such  as  this. 

There  is  also  a  growing  need  to  introduce  an  Acoustic  modem  to  the  ARIES 
platform.  With  its  accurate  navigational  system  in  place,  it  is  ready  to  take  on  the  role  of 
a  mobile  server  for  communicating  with  other  AUVs.  This  idea,  suggested  for  the 
AIRES  by  Prof.  Anthony  J.  Healey,  would  use  one  vehicle  as  a  server  to  provide  position 
information,  and  “mission  reconfigurable  commands.”  “Multi-vehicle  Networks”,  have 
the  potential  to  be  very  useful  to  the  mine  countermeasure  community  in  the  U.S.  Navy 
as  well.  Allowing  fleets  of  AUVs,  all  communicating  through  a  server  vehicle,  to  clear 
paths  for  Navy  surface  ships  or  Marine  amphibious  units,  could  save  a  lot  of  time, 
money,  and  personnel. 
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APPENDIX  A:  NMEA  MESSAGE  FORMAT  [Ref.  5] 


This  appendix  identifies  the  selection  of  NMEA  0183  command  and  status  query 
messages  valid  for  the  ABX-3.  The  latest  version  is  available  by  contacting: 

National  Marine  Electronics  Association 
NMEA  Executive  Director 
P.  0.  Box  50040,  Mobile,  Alabama  36605,  USA 
Tel  (205)  473-1793  Fax  (205)  473-1669 

Information  on  the  RTCM  SC  104  format  is  available  at: 

Radio  Technical  Commission  for  Maritime  Services 
Post  office  Box  19087 
Washington,  DC  20036 


NMEA  MESSAGE  ELEMENTS 

All  NMEA  0183  messages  possess  a  common  structure,  including  a  message  header, 
data  fields,  and  a  terminating  carriage  return  and  line  feed. 

Example:  $GPYYY,xxx,xxx,xxx ...  <CR><LF> 


The  table  below  displays  the  elements  of  this  example  message. 


Element 

Description 

SGP 

Message  Identifier  Indicating  a  GPS  Related 

Message 

Yyy 

Type  of  GPS  Message 

Xxx 

Variable  Length  Message  Fields 

<CR> 

Carriage  Return 

<LF> 

Line  Feed 
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ABX-3  SUPPORTED  MESSAGES 


The  ABX-3  supports  the  NMEA  commands  and  queries  listed  in  the  following  Table. 


Message  description 

Description 

Commands 

SGPMSK  (Full  Manual  Tune) 

Sets  the  receiver  into  Full  Manual  Tune  Mode 

SGPMSK  (Partial  Manual  Tune) 

Sets  the  receiver  into  Partial  Manual  Tune  Mode 

SGPMSK  (ABS  Mode) 

Sets  the  receiver  into  Automatic  Beacon  Search 

Mode 

$PCSI,4  (Proprietary) 

Erases  the  Global  Search  table  forcing  a  new  search 

$PCSI,5  (Proprietary) 

Sets  the  baud  rate  of  the  ABX-3  communication 
port 

$PCSI,6  (Proprietary) 

Reserved 

$PCSI,7  (Proprietary) 

Reserved 

SPSLIB  (Proprietary) 

Sets  the  frequency  and  MSK  bit  rate  of  the  ABX-3 

Queries 

SGPCRQ  Operation  Query 

Queries  the  receiver  for  operation  parameters 

SGPCRQ  Performance  Query 

Queries  the  receiver  for  performance  parameters 

$PCSI,0  (Proprietary) 

Lists  available  proprietary  SPCSI  commands  and 
queries 

$PCSI,1  (Proprietary) 

Displays  receiver  operating  parameter  status 

$PCSI,2  (Proprietary) 

Queries  the  receiver  for  operation  parameters 

$PCSI,3  (Proprietary) 

Outputs  receiver  search  information 

NMEA  0183  COMMANDS 

This  section  discusses  the  standard  and  proprietary  NMEA  0183  commands  accepted 
by  the  ABX-3. 


•  Full  Manual  Tune  Command  (SGPMSK) 

This  command  instructs  the  ABX-3  to  tune  to  a  specified  frequency  and  MSK 
Rate.  It  has  the  following  form: 

$GPMSK,fff.f,M,ddd,M,n<CR><LF> 

•  Partial  Manual  Tune  Command  (SGPMSK) 

This  command  instructs  the  ABX-3  to  tune  to  a  specified  frequency  and 
automatically  select  the  correct  MSK  rate.  It  has  the  following  form: 

SGPMSK, fff.f,M„A,n<CR><LF> 

•  Automatic  Beacon  Search  Command  (SGPMSK) 

This  command  initiates  the  ABX-3  automatic  mode  of  operation  in  which  the 
receiver  operates  without  operator  intervention,  selecting  the  most  appropriate  beacon 
station.  This  command  has  the  following  format: 

$GPLMSK„A„A,n<CR><LF> 

•  Wipe  Search  Command  ($PCSI,4) 
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The  Wipe  Search  command  instructs  the  ABX-3  to  erase  all  parameters  within  the 
beacon  almanac  and  to  initiate  a  new  Global  Search  to  identify  the  beacon  signals 
available  for  a  particular  area.  The  command  has  the  following  form: 

$PCSI,4<CR><LF> 

•  Baud  Rate  Change  Command  ($PCSI,5) 

This  proprietary  $PCSI  command  is  reserved  for  use  with  the  ABX-3. 

$PCSI,5,r<CR><LF> 

•  Tune  Command  (SPSLIB) 

A  majority  of  Garmin  hand-held  and  fixed-mount  GPS  receivers  output  this  non¬ 
standard  command  from  the  BEACON  RCVR  feature  of  the  INTERFACE  menu.  It 
instructs  both  the  connected  beacon  receiver  to  tune  to  the  specified  frequency  and  MSK 
Rate.  The  command  has  the  following  form: 

SPSLIB, fff.f,ddd<CR><LF> 


NMEA  0183  QUERIES 

This  section  discusses  the  standard  and  proprietary  NMEA  0  1  83  queries  accepted  by 
the  ABX-3  receiver. 


•  Receiver  Operating  Status  Query  (SGPCRQ) 

This  standard  NMEA  query  prompts  the  ABX-3  receiver  for  its  operational  status. 
It  has  the  following  format: 

SGPCRQ, MSK<CR><LF> 


•  Receiver  Performance  Status  Query  (SGPCRQ) 

This  standard  NMEA  query  prompts  the  ABX-3  receiver  for  its  performance 

status: 

SGPCRQ, MSS<CR><LF> 


•  Receiver  Help  Query  ($PCSI,C) 


This  command  queries  the  ABX-3  receiver  for  a  list  of  valid  proprietary  SPCSI 
commands: 


SPCSI,  0<CR><LF> 


•  ABX-3  Status  Line  A,  Channel  0  (SPCSI,  1) 

This  commands  the  ABX-3  to  output  a  selection  of  parameters  related  to  the 
operational  status  of  its  primary  channel.  It  has  the  following  format: 

SPCSI,  1<CR><LF> 

•  ABX-3  Status  Line  B,  Channel  1  (SPCSI, 2) 
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This  commands  the  ABX-3  to  output  a  selection  of  parameters  related  to  the 
operational  status  of  its  secondary  channel.  It  has  the  following  format: 

$PCSI,2<CR><LF> 

•  Receiver  Search  Dump  ($PCSI,3) 

This  query  commands  the  ABX-3  to  display  the  search  information  used  for 
beacon  selection  in  Automatic  Beacon  Search  mode.  The  output  has  three  frequencies  per 
line. 

$PCSI,3<CR><LF> 
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APPENDIX  B:  MATLAB  CODE  ASSOCIATED  WITH 
ANALYSIS  OF  INITIAL  GPS  EVALUATION 

Basic  GPS  Analysis: 

%  B.M.  STINESPRING 

%  GPS  DATA  PLOTTING  PROG 

clc 

format  long 
clear 

load  GPS021 100_2POS.d 


%  IDENTIFICATION 
GPS=GPS021 100_2POS; 

GPS_ID=GPS(:,1); 

LatDeg=GPS(:,2); 

LongDeg=GPS(:,3); 

Diff=GPS(:,4); 

NumSats=GPS(:,5); 

Time=GPS(:,6); 

Lat=GPS(:,7); 

Long=GPS(:,8); 

Altit=GPS(:,9); 

TruTrac=GPS(:,10); 

SOG=GPS(:,ll); 

VertVel=GPS(:,12); 

PDOP=GPS(:,13); 

HDOP=GPS(:,14); 

VDOP=GPS(:,15); 

TDOP=GPS(:,16); 

%  DATA  SEPARATION 

Data=[LatDeg,LongDeg,NumSats,PDOP,HDOP,VDOP5TDOP5DifF,GPS_ID]; 

P=length(Data); 

n=0; 

for  R=1  :P; 

T=R-n; 

if  Data(R,2)~=0; 

Pos(T, :  )=Data(R, :); 
else 
n=n+l; 
end 
end 
k=0; 
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U=length(Pos); 
for  S=1:U; 

U=S-k; 

ifPos(S,8)=l; 

Pos_diff(U,  :)=Pos(S , :); 
else 
k=k+l; 
end 
end 

Q=length(Pos_diff) ; 

Dop_average=mean(Pos_diff( :  ,4)) 

Dop_max=:max(Pos_diff(:,4)) 

NumSats_average=mean(Pos_diff(:,3)) 

Number_of_recorded_entries=P 

Number_of_fixes=U 

Number_of_fixes_with_Diff=Q 

Number_of_unusable_recorded_data=P-Q 

Percent_of_usable_data=(Q/P)*  1 00 

Geo(:,l)=Pos_diff(:,l)-(min(Pos_diff(:,l))); 

Geo(:,2)=Pos_diff(:,2)-(min(Pos_diff(:,2))); 

Geo(:,3)=transpose(l:(length(Geo))); 

%  MANIPULATION 

lambda=(36.6)*(pi/l  80); 
Lat_meters=(Geo(:,l)).*(60*  1 852); 
Long_meters=(Geo(:,2)).*(60*  1 852*(cos(lambda))); 

%  Plots 

figure(l); 

plot(Long_meters,Lat_meters,'-bs','LineWidth',  1 
'MarkerEdgeColorVk',... 
'MarkerFaceColor','r',... 

'MarkerSize',3) 

xlabel('<—  West  (meters)  East  -->') 
ylabel('<--  South  (meters)  North  — >') 
grid 

title('Geographic  Plot  of  GPS  Track'); 

F  in=  [Lat_meters,Long_meters,Pos_diff( :  ,9)] ; 
A=Fin(l,2); 

B=Fin(l,l); 

C=F  in((length(F  in)),2); 

D=F  in((length(F  in)),  1 ) ; 
text(A,B,'\lefitarrow  StartVFontSize',8) 
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text(C,D,'\leftarrow  Finish','FontSize',8) 
a=Pos_diff(:,9); 
for  b=l  :length(a); 
c=Fin(b,2); 
d=Fin(b,l); 

plot_text=num2str(a(b)); 

text(c,d,plot_text); 

end; 

figure(2); 

E=Pos_diff(:,3); 

F=2:l:10; 

hist(EJF); 

title('  Satelite  Frequency  (Open  Water)') 
xlabel('Number  of  Satelites') 
ylabel('Frequency') 
grid; 

figure(3) 

G=transpose(l:Q); 

plot(G,Pos_diff( :  ,4),G,Pos_diff( : ,  5 ), . . . 

G,Pos_diff(:,6),G,Pos_diff(:,7)) 
legend('PDOP7HDOP','VDOP7TDOP'); 
title('DOP  vs  Interval  (Open  Water)') 
grid 

figure(4) 

subplot(3,l,l),  bar(GPS_ID,NumSats),grid,colormap  summer; 

ylabelCNumber  of  Sats'); 

subplot(3,l,2), 

plot(GPS_ID,PDOP,GPS_ID,HDOP,GPS_ID,VDOP,GPS_ID,TDOP),grid; 
legend('PDOP','HDOP','VDOPVTDOP'); 
subplot(3,l,3),  bar(GPS_ID,Diff),grid,colormap  jet; 
ylabel('Differential  Signals'); 


figure(5) 

plot(Long_meters,Lat_meters5'-bs','LineWidth',l,.- 

'MarkerEdgeColor','r',... 

'MarkerFaceColor','r',... 

'MarkerSize',2) 

xlabel('<--  West  (meters)  East  — >') 
ylabel('<—  South  (meters)  North  — >') 
grid 

title('Geographic  Plot  of  GPS  Track  (Open  Water)'); 
Fin=[Lat_meters,Long_meters,Pos_diff(:,9)]; 
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A=Fin(l,2); 

B=Fin(l,l); 

C=Fin((length(Fin)),2); 
D=Fin((length(Fin)),  1); 
text(A,B,'\leftarrow  Start','FontSize',8) 
text(C,D,'\leflarrow  Finish', 'FontSize', 8) 
legend('DGPS  Fix  on  AUV  Track'); 

figure(6) 

horz=Fin(  1 43 : 1 64,2); 
vert=Fin( 1 43 : 1 64, 1 ); 
[P,S]=polyfit(horz,vert,  1 ); 

[Y, Delta]  =Poly  V  al(P,horz,  S); 
Standard_Deviation_of_Error=std(Delta) 
Mean_of_Error=mean(Delta) 
plot(horz,vert,'* ',horz,  Y,'-') 
axis([-4,-3,2,9]); 

legend('Data  Points', 'Linear  Regression') 
title('Linear  Regression  Plot') 
xlabel('<—  West  (meters)  East  — >') 
ylabel(’<—  South  (meters)  North  — >') 
Finny=Fin(143:164,:); 
aa=Finny(:,3); 
for  bb=l  :length(horz); 
cc=Finny(bb,2); 
dd=Finny(bb,l); 
plott_text=num2str(aa(bb)); 
text(cc,dd,plott_text); 
end; 
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APPENDIX  C:  MATLAB  CODE  ASSOCIATED  WITH 
ARIES  NAVIGATIONAL  FILTER 


1st  File  for  Variable  Assignment: 

Navjd  =  Nav(:,l); 

Hour  =  Nav(:,2); 

Minute  =  Nav(:,3); 

Second  =  Nav(:,4); 

MP_Id  =  Nav(:,5); 

MPjp  =  Nav(:,6); 

MP_q  =  Nav(:,7); 

MP_r  =  Nav(:,8); 

MP_XAccel  =  Nav(:,9); 

MP_Y Accel  =  Nav(:,10); 
MP_ZAccel  =  Nav(:,l  1); 

RDI_Id  =  Nav(:,12); 

RDI_Ug  =  Nav(:,13); 

RDI_Vg  =Nav(:,14); 

RDI_Wg  =Nav(:,15); 

RDI_Uf  =Nav(:,16); 

RDI_Vf  =  Nav(:,17); 

RDI_Wf  =  Nav(:,18); 

RDIAlt  =Nav(:,19); 
RDI_Heading  =  Nav(:,20); 

GPS_Id  =  Nav(:,21); 

Diff =Nav(:,22); 

NSv  =  Nav(:,23); 

ToC  =  Nav(:,24); 

Lat  =  Nav(:,25); 

LatDeg  =  Nav(:,26); 

Long  =  Nav(:,27); 

LongDeg  =  Nav(:,28); 

SbA  =  Nav(:,29); 

TtTc  =Nav(:,30); 

SoG  =  Nav(:,31); 

Vv  =  Nav(:,32); 

Pdop  =  Nav(:,33); 

Hdop  =  Nav(:,34); 

Vdop  =  Nav(:,35); 

Tdop  =  Nav(:,36); 

DepthRaw  =  Nav(:,37); 

Depth  =  Nav(:,38); 

Depth_dot  =  Nav(:,39); 
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2nd  File  for  Variable  Assignment: 


clear 

clc 

load  d033000_05.d 

(where  d033000_05.d  is  a  data  file) 

d=d033000_05; 

State_Id  =d(:,l); 

Navjd  =  d(:,2); 
t  =d(:,3); 

X  =  d(:,4); 

Y  =  d(:,5); 

Depth  =  d(:,6); 

Alt  =  d(:,7); 
phi  =  d(:,8); 
theta  =  d(:,9); 
psi  =  d(:,10); 
u  =  d(:,ll); 

v  =d(:,12); 

Depth_dot  =  d(:,13); 

Alt_dot  =  d(:,14); 

P  =d(:,15); 

q  =d(:,16); 

r  =  d(:,17); 

n_ls  =  d(:,18); 
n_rs  =d(:,19); 
n_bvt  =  d(:,20); 
n_svt  =  d(:,21); 
n_blt  =  d(:,22); 
n_slt  =  d(:,23); 
delta_r  =  d(:,24); 
delta_sp  =  d(:,25); 


Basic  Navigational  Filter  File: 


%  Navigation  Filter 
%  For  ARIES  AUV  Navigation  Data 

clear 

clc 

load  d033000  04.d 


assign 


%  Assign  Y  matrix  from  "assignNav"  output 
y=[u,v,psi,r,X,Y]; 

%  Define  Filter  variables 

ug=y(:,l); 

vg=y(:,2); 

heading=y(:,3); 

yaw_rate=y(:,4); 

lat=y(:,5); 

long=y(:,6); 

wg=Depth_dot; 

%%%%%  Defining  Sample  Size  %%%%% 

startSample=l ; 

endSample=length(ug); 


%%%%%  Converting  the  Lat  and  Long  Data  %%%%% 
long=long*60*3600*.5 1  *0.80; 

1 1  =long(startSample); 

lat=lat*  60  * 3 600 * . 5 1 ; 

12=lat(startSample); 

long=long-l  1  .*ones(length(lat),  1 ); 
lat=lat-12  *  ones(length(lat),  1 ); 


%%%%%  Setting  the  Time  base  %%%%% 
dt=.1235; 

t=0 :  dt:(length(ug)- 1  )*dt; 

%t=0:.  1 25  :((length(yaw_rate)- 1  )/8); 

%%%%%  Adjusting  Heading  for  360  degree  wrapping  %%%%% 
nheading  =  zeros(l,  length(heading)); 
nheading(l)  =  heading(l); 
for  i=2:length(heading) 
if  abs(heading(i)  -  heading(i-l))  <=  180 
nheading(i)  =  nheading(i-l)  +  heading(i)  -  heading(i-l); 
end 

if  heading(i)  -  heading(i-l)  >  180 
nheading(i)  =  nheading(i-l)  +  heading(i)  -  heading(i-l)  -  360; 
end 

if  heading(i-l)  -  heading(i)  >  180 
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nheading(i)  =  nheading(i-l)  +  heading(i)  -  heading(i-l)  +  360; 
end 
end 

heading  =  ((nheading')- 1 8)*(pi/l  80)+2*pi;  %Must  be  in  radians 


%%%%%  Defining  Y(  1,2, 3,5, 6)  %%%%% 
y=[ug,vg, heading, yaw_rate,lat, long]; 


%%%%%  Initializing  the  Loop  %%%%% 
psiO=mean(heading(startSample :  startSample+ 15)); 

%iniialize  the  state  vector, x  is  8,y  is  6 

x=zeros(8,endSample);err=zeros(6,endSample); 

s=startSample, 

endSample 

x(:,s)=[lat(s),long(s),psi0,yaw_rate(s),ug(s),vg(s),0,0]'; 

w=l; 

%MANEUVERING  AND  CURRENT  TIME  CONSTANTS 

%Initial  A  matrix 
A=zeros(8,8); 

X=x(l,s);Y=x(2,s);psi=x(3,s);r=x(4,s);  dop_ug=x(5,s);dop_vg=x(6,s); 
br=x(7,s);bpsi=x(8,s); 

A(l,3)=-dop_ug*sin(psi)-dop_vg*cos(psi); 

A(l,5)=cos(psi); 

A(l,6)=-sin(psi); 

A(2,3)=dop_ug*cos(psi)-dop_vg*sin(psi); 

A(2,5)=sin(psi); 

A(2,6)=cos(psi); 

A(3,4)=l; 

%A(8,4)=0.1; 

%  x(:,s)=[lat(s),long(s),psi0,yaw_rate(s)*pi/180,dop_ug(s),dop_vg(s),br,bpsi]'; 
%  y=[ug,vg,  compass,  yawrate,gpsX,  gpsY} 

%Initial  C  matrix 
%yaw_rate=r+br; 

%heading=psihat+bpsi ; 

C=zeros(6,8); 

C(l,5)=l; 

C(2,6)=l; 

C(3,3)=1;C(3,8)=1; 
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C(4,4)=l  ;C(4,7)=1 ; 
C(5,1)=1;C(6,2)=1; 


%Initial  B  matrix 

ql=0;%variance  on  X,  mA2 

q2=0;%variance  on  Y,  mA2 

q3=0.001;%%  variance  on  psi,  radA2 

q4=0.001;%  variance  on  r,  rad/s)A2%  0.01  is  normal 

q5=0.01;%  variance  on  ug,(m/s)A2 

q6=0.01;%  variance  on  vg,(m/s)A2 

q7=0.000001; 

q8=0.000001; 

B=[ql;q2;q3;q4;q5;q6;q7;q8]; 

%B=[0;0;0;0;0;0;0;0;0;0]; 

Q=diag(B); 

%system  noise 

%nul=.  01  ;nu2=.01  ;nu3=.  1  ;nu4=0.001;nu5=l  .0;nu6=l  .0; 
nul=.01  ;nu2=.  01  ;nu3=0. 1  ;nu4=0.001  ;nu5=l  .0;nu6=l  .0; 
gnu=[nul  ;nu2;nu3  ;nu4;nu5  ;nu6]*  1 0; 

R=diag(gnu);  %  measurement  noise 
psave=zeros(8,endSample); 

%Note,  old_after  means  measured  data  at  old  time,  new_before  means  model  predicted 

value 

%load  old; 

p_old_after=diag([0. 1  .1  .1  .1  .1  .1  .001  ,001])/10; 

delx_old_after=zeros(8, 1 ); 

g=ones(8,l); 

p_old_after(8,8)=0.0 1 ; 

for  i=startSample:(endSample-l); 

%compute  linearized  PHI  matrix  using  updated  A 
phi=expm(A*dt); 

%reset  initial  state 
x_old_after=x(:,i); 

%  nonlinear  state  propagation 
[x_new_before]=prop8_new(x_old_after,0,0,dt,0); 

%error  covariance  propagation 
p_new_before=phi*p_old_after*phi'  +  Q; 

%new  gain  calculation  using  linearized  new  C  matrix  and  current  state 
%error  covariances. 

%formulate  the  innovation  using  nonlinear  output  propagation 
%as  compared  to  new  sampled  data  from  measurements 
yhat=output8_new(x_new_before); 
err(l:6,i+l)=(y(i+l,l:6)'  -  yhat); 
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%  Restrictions  on  C 
if  sqrt(err(5,i+ 1  )A2+err(6,i+ 1  )A2)>  1000, 
err(5,i+l)=0;err(6  ,i+l)=0; 

end; 

if  y(i+ 1  >  1  )==y(is  1  )»C(  1  ,:)=0.0*g';end; 
if  (abs(y(i+l,l)))>10,C(l,:)=0.0*g';end; 
ify(i+l,2)==y(i,2),C(2,:)=0.0*g';end; 
if  (abs(y(i+l,2)))>10,C(2,:)=0.0*g';end; 
ify(i+l,3)==y(i,3),C(3,:)=0.0*g’;end; 
if  y(i+l,4)==y(i,4),C(4,:)=0.0*g';end; 

%%%%%  GPS  RESTRICTIONS  %%%%% 

%  timed  fixes  (take  GPS  fixes  for  10  seconds  every  3  min) 
%p=[startSample:  1 440 : 1 E6] ; 

%f=p(w); 

%v=i-f; 

%if  v==1440;w=w+l  ;end 

%ifv>=80;C(5,:)=0.0*g';C(6,:)=0.0*g'; 

%else  C(5,l)=l  ;C(6,2)=1  ;end 

%  heading  triggered  fixes  (take  GPS  fix  for  90  degree  change) 
%k=(abs(y(i+l,3)-y(i,3))); 

%if  k>=(pi/l  000);C(5, 1  )=1  ;C(6,2)=1  ;end; 

%ifi<=1000,  C(5 , 1  )=  1  ;C(6,2)=  1  ;end; 

%if  i>l  000,  C(5,:)=0.0*g';C(6,:)=0.0*g';end; 

%if  i>=1200,  C(5,l)=l;C(6,2)=l;end; 
ify(i+l,5)==y(i,5),C(5,:)=0.0*g';end; 
ify(i+l,6)==y(i,6),C(6,:)=0.0*g-;end; 


%eliminate  dgps  input 
cpc=C*p_new_before*C'+R; 
rel(:  ,i+ 1  )=err( :  ,i+ 1 )’ *  inv(cpc)  *  err(:  ,i+ 1 ) ; 

%  compute  gain,  update  toal  state  and  error  covariance 
G=p_new_before*C'*inv(C*p_new_before*C'  +  R);  %  Kalman  Gain 
p_old_after=[eye(8)  -  G*C]*p_new_before; 
psave( :  ,i+ 1  )=diag(p_old_after) ; 

%if  gpsStatus(i+l)~=2,  G=zeros(8,6);end; 
x_new_after=x_new_before  +  G*err(:,i+1); 

%carry  new  state  into  next  update 
x(:  ,i+ 1  )=x_new_after; 

%resetting  up  the  linearized  A  matrix 
A=zeros(8,8); 
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X=x(l  ,i+ 1 );  Y=x(2,i+1  );psi=x(3  ,i+ 1  );r=x(4,i+ 1 ); 
dop_ug=x(5,i+ 1 )  ;dop_vg=x(6,i+ 1  );br=x(7,i+ 1 )  ;bpsi=x(8  ,i+ 1 ) ; 

A=zeros(8,8); 

A(  1 ,3)=-dop_ug*sin(psi)-dop_vg*cos(psi); 

A(l,5)=cos(psi); 

A(l,6)=-sin(psi); 

A(2,3)=dop_ug*  cos(psi)-dop_vg*  sin(psi); 

A(2,5)=sin(psi); 

A(2,6)=cos(psi); 

A(3,4)=l; 

%A(8,4)=0. 1 ;  %reset  the  linearized  C  matrix 
C=zeros(6,8); 

C(l,5)=l; 

C(2,6)=l; 

C(3,3)=1;C(3,8)=1; 

C(4,4)=l  ;C(4,7)=1 ; 

C(5,1)=1;C(6,2)=1; 

end; 

%%%%%  DEAD-RECKONING  SOLUTION  GENERATION  %%%%% 

Xdr=zeros(  1  ,length(t));  Y  dr=Xdr;psi2=Xdr;psi2(startSample)=heading(startSample); 
for  i=startSample:(endSample-l), 
bias=0.487551 55037954  le-002; 

%bias=x(7,i-l); 

psi2(i+ 1  )=psi2(i)+(yaw_rate(i)-bias)* 0. 1235; 

%ppsi=heading(i)*pi/l 80-20*cos(heading(i)).*pi/l  80;  %-x(8,i); 

%if  t(i)>520;  ppsi=heading(i)*pi/180+4*pi/180;end; 

%ppsi=psi2(i); 

ppsi=heading(i);  %%%%%%%%% 
pphi=0;  %phil(i);  %MP_YAccel(i)/9.81; 
pth=0;  %theta(i);  %MP_XAccel(i)/9.81; 
if  (abs(y(i,  1  )))>  1 0,ug(i)=ug(i- 1  );end; 
if  (abs(y(i,2)))>  1 0,vg(i)=vg(i- 1  );end; 


fl(i)=ug(i)*cos(ppsi)*cos(pth)-vg(i)*(sin(ppsi)*cos(pphi).„ 

+cos(ppsi)*sin(pth)*sin(pphi))+sin(ppsi)*sin(pphi)*wg(i); 

f2(i)=ug(i)*sin(ppsi)*cos(pth)+vg(i)*(cos(ppsi)*cos(pphi)+sin(ppsi)*sin(pth)*sin(pphi))- 

cos(ppsi)*sin(pphi)*wg(i); 


Xdr(i+ 1  )=Xdr(i)+0. 1 23 5 *  (fl  (i)); 
Y  dr(i+ 1  )= Y  dr(i)+0. 1 23 5  *  f2(i) ; 
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end; 


figure(l),clf 

plot(t(startSample:endSample),x(3,startSample:endSample)*  180/pi, 'y.',... 
t(startSample:endSample),y(startSample:endSample,3)*l  80/pi, 'g',... 
t(startSample:endSample),psi2(startSample:endSample)*  1 80/pi,'r.'),grid 

title('FIGURE  1  est.  heading, y,  compass,  g') 

%plot(heading,'g'),grid 

title('FIGURE  1  est.  heading, y,  compass,  g') 

zoom 

figure(2),clf 

plot(y(startSample:endSample,6),y(startSample:endSample,5),'bo'),grid 
hold  on 

plot(x(2,startSample:endSample),x(l,startSample:endSample),'g.') 
plot(Ydr(startSample:endSample),Xdr(startSample:endSample),'y.') 
title('Figure  2  Estimated  Path,  g  ,  DGPS  Data,  c  Dead  Reckoning,  y ') 
xlabel('X,  meters');ylabel('Y,  meters’);axis([-100  200  -100  200]); 
hold  off 
zoom 

figure(3) 

plot(t(startSample:endSample),x(7,startSample:endSample),'g'),grid 

hold 

%plot(t(startSample:endSample),x(9,startSample:endSample),'r.') 
title('r  bias  estimate,  g') 
hold  off 
zoom 

figure(4) 

plot(t(startSample:endSample),x(8,startSample:endSample)*  180/pi, 'g.'), grid 
title('compass  bias  estimate,  g.') 

figure(5)  %doppler  u 

plot(t(startSample:endSample),ug(startSample:endSample),'y+',... 
t(startSample:endSample),x(5,(startSample:endSarnple)),'c*'),grid 
title('Doppler  u,  +  and  estimated  u,  *  vs  Time  sec') 
zoom 

figure(6) 

plot(psave(  1 ,:),'.  -')  ;hold;plot(psave(2, :)) ; 

grid;  title('First  and  Second  Covariance  Elements'); 

legend('First  Element', 'Second  Element'); 

figure(7) 
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plot(ug);  title(' Acoustic  Doppler  Forward  Speed  (ug)'); 
grid; 

figure(8) 

plot(vg);  title(Acoustic  Doppler  Lateral  Speed  (vg)'); 
grid 

figure(9) 

plot(RDI_Alt);hold 

plot(NSv,'m.');grid;title('Satellite  Recognition  Response'); 
xlabel('Time');ylabelCNumber  of  Satellites  and  Altitude  (meters)'); 

figure(lO) 

plot(RDI_Alt);hold;plot(ug+7,'r');grid;title('Diving  Speed'); 
xlabel('Time');ylabel('Depth  (meters)  and  Speed+7  (meters/sec)'); 


1st  Accompanying  Navigational  Filter  File: 

function  [xnew]=  Prop8_new(xold,tau,taul,tl,t2) 

X=xold(  1 ) ;  Y =xold(2);psi=xold(3 ) ; 

r=xold(4);ug=xold(5);vg=xold(6); 

br=xold(7);bpsi=xold(8); 

fl  =ug*cos(psi)-vg*  sin(psi); 

f2=ug*  sin(psi)+vg*  cos(psi) ; 

f3=r; 

f4=0;  £5=0;  f6=0; 
f=[fl  ;f2;f3;f4;f5;f6;0;0]; 
xnew=xold+f.  *  (t  1  -t2);%xd=f; 


2nd  Accompanying  Navigational  Filter  File: 

function  [yhat]=output8_new(xold) 

X=xold(  1 );  Y=xold(2);psi=xold(3); 
r=xold(4)  ;ug=xold(5);vg=xold(6) ; 
br=xold(7)  ;bpsi=xold(8) ; 
y  1  =ug;y2=vg;y3=psi+bpsi;y4=r+br;y5=X;y6=Y ; 
yhat=[y  1  ;y2;y3;y4;y5;y6]; 
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